def getIntegralDOF(th, d_th, dt): new_th = [None] * len(th) v_r0 = d_th[0][0:3] v_r1 = d_th[0][3:6] new_th0_l = th[0][0] + [v_r0[0] * dt, v_r0[1] * dt, v_r0[2] * dt] new_th0_a = np.dot(th[0][1], mm.exp([v_r1[0] * dt, v_r1[1] * dt, v_r1[2] * dt])) new_th[0] = [None] * 2 new_th[0][0] = new_th0_l new_th[0][1] = new_th0_a for i in range(1, len(th)): new_th[i] = np.dot( th[i], mm.exp([d_th[i][0] * dt, d_th[i][1] * dt, d_th[i][2] * dt])) return new_th
def extendByIntegration(motion, extendLength, preserveJoints=[], finiteDiff=1): lastFrame = len(motion) - 1 p = motion.getJointPositionGlobal(0, lastFrame) v = motion.getJointVelocityGlobal(0, lastFrame - finiteDiff, lastFrame) a = motion.getJointAccelerationGlobal(0, lastFrame - finiteDiff, lastFrame) ap = motion.getJointOrientationsLocal(lastFrame) av = motion.getJointAngVelocitiesLocal(lastFrame - finiteDiff, lastFrame) aa = motion.getJointAngAccelerationsLocal(lastFrame - finiteDiff, lastFrame) t = 1 / motion.fps # integration extended = ym.JointMotion( [motion[0].getTPose() for i in range(extendLength)]) for i in range(extendLength): p += v * t v += a * t ap = map(lambda R0, dR: np.dot(R0, mm.exp(t * dR)), ap, av) av = map(lambda V0, dV: V0 + t * dV, av, aa) extended[i].rootPos = p.copy() extended.setJointOrientationsLocal(i, ap) # preserve joint orientations preserveJointOrientations = [ motion[-1].getJointOrientationGlobal(footJoint) for footJoint in preserveJoints ] for extendedPosture in extended: for i in range(len(preserveJoints)): extendedPosture.setJointOrientationGlobal( preserveJoints[i], preserveJointOrientations[i]) return extended
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 transToSE3(trans): v = Vec3(trans[0], trans[1], trans[2]) length = mm.length(trans[3:]) if length < LIE_EPS: return SE3(v) R = mm.exp(trans[3:] / length, length) return SE3(R[0, 0], R[1, 0], R[2, 0], R[1, 0], R[1, 1], R[1, 2], R[2, 0], R[2, 1], R[2, 2], trans[0], trans[1], trans[2])
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 addJointSO3FromBvhJoint(self, jointPosture, bvhJoint, channelValues, scale=1.0): localR = mm.I_SO3() local_t = mm.O_Vec3() for channel in bvhJoint.channels: if channel.channelType == 'XPOSITION': # jointPosture.rootPos[0] = channelValues[channel.channelIndex]*scale local_t[0] = channelValues[channel.channelIndex] * scale elif channel.channelType == 'YPOSITION': # jointPosture.rootPos[1] = channelValues[channel.channelIndex]*scale local_t[1] = channelValues[channel.channelIndex] * scale elif channel.channelType == 'ZPOSITION': # jointPosture.rootPos[2] = channelValues[channel.channelIndex]*scale local_t[2] = channelValues[channel.channelIndex] * scale elif channel.channelType == 'XROTATION': localR = numpy.dot( localR, mm.exp(mm.s2v((1, 0, 0)), mm.deg2Rad(channelValues[channel.channelIndex]))) elif channel.channelType == 'YROTATION': localR = numpy.dot( localR, mm.exp(mm.s2v((0, 1, 0)), mm.deg2Rad(channelValues[channel.channelIndex]))) elif channel.channelType == 'ZROTATION': localR = numpy.dot( localR, mm.exp(mm.s2v((0, 0, 1)), mm.deg2Rad(channelValues[channel.channelIndex]))) # jointPosture.setLocalR(bvhJoint.name, localR) jointPosture.setLocalR( jointPosture.skeleton.getElementIndex(bvhJoint.name), localR) jointPosture.setLocal_t( jointPosture.skeleton.getElementIndex(bvhJoint.name), local_t) for i in range(len(bvhJoint.children)): self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i], channelValues, scale)
def get_th_dart(skel): ls = [] pyV = np.asarray(skel.q[3:6]) pyR = mm.exp(np.asarray(skel.q[:3])) ls.append([pyV, pyR]) for i in range(1, len(skel.joints)): joint = skel.joints[i] if joint.num_dofs() > 0: ls.append(joint.get_local_transform()[:3, :3]) return ls
def makeFourContactPos(posture, jointNameOrIdx, isLeftFoot=True, isOutside=True): """ :type posture: ym.JointPosture :type jointNameOrIdx: str | int :return: np.array, np.array, np.array """ idx = jointNameOrIdx if type(jointNameOrIdx) == str: idx = posture.skeleton.getJointIndex(jointNameOrIdx) insideOffset = SEGMENT_FOOT_MAG * np.array((0., 0., 2.5)) outsideOffset = SEGMENT_FOOT_MAG * np.array((1.2, 0., 2.5)) if isLeftFoot ^ isOutside: # if it is not outside phalange, outsideOffset[0] = -1.2 * SEGMENT_FOOT_MAG origin = posture.getJointPositionGlobal(idx) inside = posture.getJointPositionGlobal(idx, insideOffset) outside = posture.getJointPositionGlobal(idx, outsideOffset) length = SEGMENT_FOOT_MAG * 2.5 RotVec1_tmp1 = inside - origin RotVec1_tmp2 = inside - origin RotVec1_tmp2[1] = 0. RotVec1 = np.cross(RotVec1_tmp1, RotVec1_tmp2) angle1_1 = math.acos((origin[1] - SEGMENT_FOOT_RAD) / length) angle1_2 = math.acos((origin[1] - inside[1]) / length) footRot1 = mm.exp(RotVec1, angle1_1 - angle1_2) footOri1 = posture.getJointOrientationGlobal(idx) posture.setJointOrientationGlobal(idx, np.dot(footRot1, footOri1)) inside_new = posture.getJointPositionGlobal(idx, insideOffset) outside_new_tmp = posture.getJointPositionGlobal(idx, outsideOffset) # RotVec2 = inside_new - origin width = np.linalg.norm(outside - inside) widthVec_tmp = np.cross(RotVec1_tmp1, np.array((0., 1., 0.))) if isLeftFoot ^ isOutside \ else np.cross(np.array((0., 1., 0.)), RotVec1_tmp1) widthVec = width * widthVec_tmp / np.linalg.norm(widthVec_tmp) outside_new = inside_new + widthVec footRot2 = mm.getSO3FromVectors(outside_new_tmp - inside_new, widthVec) footOri2 = posture.getJointOrientationGlobal(idx) # print footRot2, footOri2 posture.setJointOrientationGlobal(idx, np.dot(footRot2, footOri2)) return
def continue_from_now_by_time(self, t, prev_t=0.): # self.phase_frame = frame # t = frame /self.ref_motion.fps motion_q = self.ref_motion.get_q_by_time(t) motion_ori = mm.exp(motion_q[:3]) attach_pos = np.zeros(3) attach_ori = np.zeros(3) motion_prev_q = self.ref_motion.get_q_by_time(prev_t) motion_prev_ori = mm.exp(motion_prev_q[:3]) # attach current controlled character skel_pelvis_offset = self.skel.joint( 0).position_in_world_frame() - motion_q[3:6] # attach current motion # skel_pelvis_offset = motion_prev_q[3:6] - motion_q[3:6] skel_pelvis_offset[1] = 0. self.ref_motion.translateByOffset(skel_pelvis_offset) # attach current controlled character skel_pelvis_x = self.skel.joint(0).orientation_in_world_frame()[:3, 0] # attach current motion # skel_pelvis_x = motion_prev_ori[:3, 0] skel_pelvis_x[1] = 0. motion_pelvis_x = motion_ori[:3, 0] motion_pelvis_x[1] = 0. # attach current controlled character self.ref_motion.rotateTrajectory( mm.getSO3FromVectors(motion_pelvis_x, skel_pelvis_x), fixedPos=self.skel.joint(0).position_in_world_frame()) # attach current motion # self.ref_motion.rotateTrajectory(mm.getSO3FromVectors(motion_pelvis_x, skel_pelvis_x), fixedPos=motion_prev_q[3:6]) self.time_offset = -self.world.time() + t
def step(self, _action): """Run one timestep of the environment's dynamics. Accepts an action and returns a tuple (observation, reward, done, info). # Arguments action (object): An action provided by the environment. # Returns observation (object): Agent's observation of the current environment. reward (float) : Amount of reward returned after previous action. done (boolean): Whether the episode has ended, in which case further step() calls will return undefined results. info (dict): Contains auxiliary diagnostic information (helpful for debugging, and sometimes learning). """ action = np.hstack((np.zeros(6), _action[:self.skel.ndofs - 6] / 10.)) Kp_joint = np.asarray([0.0] + [self.Kp] * self.skel.getJointNum()) Kd_joint = np.asarray([0.0] + [self.Kd] * self.skel.getJointNum()) for joint_idx in range(len(self.foot_joint)): Kp_joint[1 + joint_idx] = self.Kd * exp( log(self.Kp) * _action[self.skel.ndofs - 6 + joint_idx] / 10.) Kd_joint[1 + joint_idx] = self.Kp * exp( log(self.Kd) * _action[self.skel.ndofs - 6 + joint_idx] / 20.) DOFs = self.skel.getDOFs() action_nested = ype.makeNestedList(DOFs) th_r = self.ref_skel.getDOFPositions() ype.nested(action, action_nested) for i in range(1, len(th_r)): th_r[i] = np.dot(th_r[i], mm.exp(action_nested[i])) th = self.skel.getDOFPositions() dth = self.skel.getDOFVelocities() ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kp_joint, Kd_joint) for i in range(self.step_per_frame): bodyIDs, contactPositions, contactPositionLocals, contactForces = \ self.world.calcPenaltyForce(self.bodyIDsToCheck, self.mus, self.Ks, self.Ds) self.world.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) self.skel.setDOFAccelerations(ddth_des) self.world.step() self.update_ref_skel(False) return tuple([self.state(), self.reward(), self.is_done(), dict()])
def readTrcFile(trcFilePath, scale=1.0): f = open(trcFilePath) fileLines = f.readlines() pointMotion = ym.Motion() i = 0 while i != len(fileLines): splited = fileLines[i].split() boneNames = [] if i == 2: dataRate = float(splited[0]) numFrames = int(splited[2]) numMarkers = int(splited[3]) # print numFrames, numMarkers elif i == 3: markerNames = [name.split(':')[1] for name in splited[2:]] skeleton = ym.PointSkeleton() for name in markerNames: skeleton.addElement(None, name) # print markerNames elif i > 5: markerPositions = splited[2:] # print markerPositions # print 'i', i pointPosture = ym.PointPosture(skeleton) for m in range(numMarkers): point = numpy.array([ float(markerPositions[m * 3]), float(markerPositions[m * 3 + 1]), float(markerPositions[m * 3 + 2]) ]) point = numpy.dot( mm.exp(numpy.array([1, 0, 0]), -math.pi / 2.), point) * scale # pointPosture.addPoint(markerNames[m], point) pointPosture.setPosition(m, point) # print 'm', m # print markerNames[m], (markerPositions[m*3],markerPositions[m*3+1],markerPositions[m*3+2]) pointMotion.append(pointPosture) i += 1 f.close() pointMotion.fps = dataRate return pointMotion
def set_action(self, _action): action = np.hstack((np.zeros(6), _action/10.)) th_action = ype.makeNestedList(self.skel.getDOFs()) ype.nested(action, th_action) th_r = self.ref_motion.getDOFPositions(self.phase_frame) th_des = [th_r[0]] for i in range(1, len(th_r)): th_des.append(np.dot(th_r[i], mm.exp(th_action[i]))) th = self.skel.getDOFPositions() dth_r = self.ref_motion.getDOFVelocities(self.phase_frame) dth = self.skel.getDOFVelocities() ddth_r = self.ref_motion.getDOFAccelerations(self.phase_frame) ddth_des = yct.getDesiredDOFAccelerations(th_des, th, dth_r, dth, ddth_r, self.Kp, self.Kd) bodyIDsToCheck = list(range(self.world.getBodyNum())) mus = [.5]*len(bodyIDsToCheck) return ddth_des, bodyIDsToCheck, mus
def preFrameCallback_Always(frame): # print(mm.rad2Deg(math.pi/6.*math.sin((frame-30)*math.pi/180.))) if frame <= start_frame: vpWorld.set_plane(0, mm.unitY(), np.zeros(3)) if frame > start_frame: if math.sin((frame - start_frame) / 360. * math.pi) > 0.: if frame < start_frame + 50: setParamVal( 'com Z offset', 0.02 * math.sin(2. * (frame - start_frame) / 360. * math.pi)) else: setParamVal('com Z offset', 0.0) if math.sin((frame - start_frame) / 360. * math.pi) > 0.: foot_viewer.check_not_all_seg() foot_viewer.check_tiptoe_all() setParamVal( 'tiptoe angle', mm.deg2Rad(10.) * math.sin( (frame - start_frame) / 360. * math.pi)) # foot_viewer.check_h_l.value(False) # foot_viewer.check_h_r.value(False) else: foot_viewer.check_all_seg() # foot_viewer.check_tiptoe_all() # foot_viewer.check_h_l.value(True) # foot_viewer.check_h_r.value(True) vpWorld.set_plane( 0, np.dot( mm.exp( -mm.unitX(), mm.deg2Rad(10.) * math.sin( (frame - start_frame) / 360. * math.pi)), mm.unitY()), np.zeros(3)) plane_list = vpWorld.get_plane_list() plane_normal = plane_list[0][0] plane_origin = plane_list[0][1] viewer.motionViewWnd.glWindow.pOnPlaneshadow = plane_origin + plane_normal * 0.001 viewer.motionViewWnd.glWindow.normalshadow = plane_normal
def step(self, _action): """Run one timestep of the environment's dynamics. Accepts an action and returns a tuple (observation, reward, done, info). # Arguments action (object): An action provided by the environment. # Returns observation (object): Agent's observation of the current environment. reward (float) : Amount of reward returned after previous action. done (boolean): Whether the episode has ended, in which case further step() calls will return undefined results. info (dict): Contains auxiliary diagnostic information (helpful for debugging, and sometimes learning). """ action = np.hstack((np.zeros(6), _action/10.)) th_action = ype.makeNestedList(self.skel.getDOFs()) ype.nested(action, th_action) th_r = self.ref_motion.getDOFPositions(self.phase_frame) th_des = [th_r[0]] for i in range(1, len(th_r)): th_des.append(np.dot(th_r[i], mm.exp(th_action[i]))) th = self.skel.getDOFPositions() dth_r = self.ref_motion.getDOFVelocities(self.phase_frame) dth = self.skel.getDOFVelocities() ddth_r = self.ref_motion.getDOFAccelerations(self.phase_frame) ddth_des = yct.getDesiredDOFAccelerations(th_des, th, dth_r, dth, ddth_r, self.Kp, self.Kd) bodyIDsToCheck = list(range(self.world.getBodyNum())) mus = [.5]*len(bodyIDsToCheck) for i in range(self.step_per_frame): bodyIDs, contactPositions, contactPositionLocals, contactForces = self.world.calcPenaltyForce(bodyIDsToCheck, mus, self.Ks, self.Ds) self.world.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) self.skel.setDOFAccelerations(ddth_des) self.skel.solveHybridDynamics() self.world.step() self.update_ref_skel(False) return tuple([self.state(), self.reward(), self.is_done(), dict()])
def step(self, target): target = self.config.x_normal.normalize_l(target) m = self.model feed_dict = {m.x: [[target]], m.prev_y: self.current_y} if self.state is not None: feed_dict[m.initial_state] = self.state # x : target x, target y => 2 # # y : foot contact=2, root transform(rotation, tx, ty)=3, root_height, joint pos=3*13=39 => 45 # y : foot contact=2, root transform(rotation, tx, ty)=3, root_height, joint pos=3*19=57 => 63 output, self.state, self.current_y = self.sess.run( [m.generated, m.final_state, m.final_y], feed_dict) output = output[0][0] output = self.config.y_normal.de_normalize_l(output) angles = copy(output[63:]) output = output[:63] contacts = copy(output[:2]) output = output[2:] # move root self.pose = self.pose.transform(output) root_orientation = mm.getSO3FromVectors( (self.pose.d[0], 0., self.pose.d[1]), -mm.unitZ()) points = [[0, output[3], 0]] output = output[4:] for i in range(len(output) // 3): points.append(output[i * 3:(i + 1) * 3]) point_offset = mm.seq2Vec3([0., -0.05, 0.]) for i in range(len(points)): points[i] = mm.seq2Vec3(self.pose.global_point_3d( points[i])) / 100. + point_offset orientations = list() for i in range(len(angles) // 3): orientations.append(mm.exp(angles[i * 3:(i + 1) * 3])) return contacts, points, angles, orientations, root_orientation
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 create_any_motion(motion): #motion yme.removeJoint(motion, 'Head', False) #yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45), False) yme.updateGlobalT(motion) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 #node.length = .2 #node.width = .12 #node.width = .2 node.width = .15 node.mass = 2. #node.mass = 1. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 #node.width = .12 node.width = .15 #node.width = .2 node.mass = 2. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} return mcfg, wcfg, stepsPerFrame, config
def create_walking_biped(): #motion motionName = 'wd2_WalkForwardNormal00.bvh' #motionName = '../motions/wd2_WalkForwardNormal00.bvh' #motionName = '../motions/wd2_WalkForwardFast00.bvh' #motionName = 'wd2_jump.bvh' #motionName = 'wd2_stand.bvh' motion = yf.readBvhFile(motionName, .01) yme.removeJoint(motion, 'Head', False) #yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.0, 0)) #motion = motion[40:-58] #motion[0:0] = [motion[0]]*20 #motion.extend([motion[-1]]*5000) motion = motion[40:] #motion[0:0] = [motion[0]]*50 #motion.extend([motion[-1]]*100) #motion.extend([motion[-1]]*100) #motion = motion[30:151] #motion = motion[30:] #motion[5:5] = [motion[5]]*30 #motion[0:0] = [motion[0]]*100 #motion.extend([motion[-1]]*5000) #motion = motion[40:41] #motion[0:0] = [motion[0]]*5000 #motion = motion[56:-248] #motion = motion[-249:-248] #motion[0:0] = [motion[0]]*10 #motion.extend([motion[-1]]*5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 #node.length = .2 node.width = .12 #node.width = .2 node.mass = 2. #node.mass = 1. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 node.width = .12 #node.width = .2 node.mass = 2. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 #stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #wcfg.timeStep = (1/1000.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} #success!! #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped_zygote_two_seg(): #motion motionName = 'wd2_tiptoe_zygote.bvh' #motionName = 'wd2_jump.bvh' #motionName = 'wd2_stand.bvh' motion = yf.readBvhFile(motionName, .01) # yme.removeJoint(motion, 'Head', False) # yme.removeJoint(motion, 'Head', False) # yme.removeJoint(motion, 'RightShoulder', False) # yme.removeJoint(motion, 'LeftShoulder1', False) # yme.removeJoint(motion, 'RightToes_Effector', False) # yme.removeJoint(motion, 'LeftToes_Effector', False) # yme.removeJoint(motion, 'RightHand_Effector', False) # yme.removeJoint(motion, 'LeftHand_Effector', False) # yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False) # yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False) yme.removeJoint(motion, 'RightFoot_Effector', False) yme.removeJoint(motion, 'LeftFoot_Effector', False) yme.addJoint(motion, 'LeftFoot', 'LeftToes', [0., 0., 0.12], False) yme.addJoint(motion, 'LeftToes', 'LeftToes_Effector', [0., 0., 0.07], False) yme.addJoint(motion, 'RightFoot', 'RightToes', [0., 0., 0.12], False) yme.addJoint(motion, 'RightToes', 'RightToes_Effector', [0., 0., 0.07], False) yme.updateGlobalT(motion) # motion.translateByOffset((0, -0.07, 0)) # motion.translateByOffset((0, -0.06, 0)) motion.extend([motion[-1]] * 300) del motion[:270] for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] # node = mcfg.getNode('Hips') # node.length = .2 # node.width = .25 node = mcfg.getNode('Hips') node.length = 4. / 27. node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .177 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .8 node.offset = (0., 0., -0.02) node = mcfg.getNode('LeftFoot') node.length = .177 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .8 node.offset = (0., 0., -0.02) node = mcfg.getNode('RightToes') node.length = .053 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .218 # node.offset = (0,0,0.1) node = mcfg.getNode('LeftToes') node.length = .053 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .218 # node.offset = (0,0,0.1) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 #stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #wcfg.timeStep = (1/1000.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200 config['Dt'] = 2. * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2. * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2. * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2. * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} config['weightMap'] = { 'RightArm': .2, 'RightForeArm': .2, 'LeftArm': .2, 'LeftForeArm': .2, 'Spine': .6, 'Spine1': .6, 'RightFoot': .2, 'LeftFoot': .2, 'Hips': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3, 'RightToes': .2, 'LeftToes': .2 } #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5} #success!! ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} ''' #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} return mcfg, wcfg, stepsPerFrame, config #=============================================================================== # biped config #=============================================================================== # motion, mesh config g_motionDirConfigMap = {} g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\ 'rootRot': mm.I_SO3()} g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \ {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\ 'rootRot': mm.exp(mm.v3(1,0,0), .01)} g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\ 'rootRot': mm.exp(mm.v3(1,0,0), .0)} g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\ 'rootRot': mm.exp(mm.v3(1,0,0), .03)} #=============================================================================== # # reloadable config #===============================================================================
def ik_analytic(posture, joint_name_or_index, new_position): if isinstance(joint_name_or_index, int): joint = joint_name_or_index else: joint = posture.skeleton.getJointIndex(joint_name_or_index) # joint_parent = posture.body.joint_parent[joint] # joint_parent_parent = posture.body.joint_parent[joint_parent] joint_parent = posture.skeleton.getParentJointIndex(joint) joint_parent_parent = posture.skeleton.getParentJointIndex(joint_parent) # B = posture.get_position(joint) # C = posture.get_position(joint_parent) # A = posture.get_position(joint_parent_parent) B = posture.getJointPositionGlobal(joint) C = posture.getJointPositionGlobal(joint_parent) A = posture.getJointPositionGlobal(joint_parent_parent) L = B - A N = B - C M = C - A # l = mathlib.length(L); # n = mathlib.length(N); # m = mathlib.length(M); l = mm.length(L) n = mm.length(N) m = mm.length(M) # a = mathlib.ACOS((l*l + n*n - m*m) / (2*l*n)) # b = mathlib.ACOS((l*l + m*m - n*n) / (2*l*m)) a = mm.ACOS((l * l + n * n - m * m) / (2 * l * n)) b = mm.ACOS((l * l + m * m - n * n) / (2 * l * m)) B_new = new_position L_new = B_new - A # l_ = mathlib.length(L_new) l_ = mm.length(L_new) # a_ = mathlib.ACOS((l_*l_ + n*n - m*m) / (2*l_*n)) # b_ = mathlib.ACOS((l_*l_ + m*m - n*n) / (2*l_*m)) a_ = mm.ACOS((l_ * l_ + n * n - m * m) / (2 * l_ * n)) b_ = mm.ACOS((l_ * l_ + m * m - n * n) / (2 * l_ * m)) # rotate joint in plane # rotV = mathlib.normalize(numpy.cross(M, L)) rotV = mm.normalize2(np.cross(M, L)) rotb = b - b_ rota = a_ - a - rotb # posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV, rotb)) # posture.rotate_global_orientation(joint_parent, mathlib.exp(rotV * rota)) posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb)) posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota)) # rotate plane # rotV2 = mathlib.normalize(numpy.cross(L, L_new)) # l_new = mathlib.length(L_new) # l_diff = mathlib.length(L_new - L) # rot2 = mathlib.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l)) # posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV2, rot2)) rotV2 = mm.normalize2(np.cross(L, L_new)) l_new = mm.length(L_new) l_diff = mm.length(L_new - L) rot2 = mm.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l)) posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2)) return posture
def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel_ik.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list( map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list( map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [ motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_joint_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_ori = list( map(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_vel = [ ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel)) ] is_contact = [1] * len(contact_ids) contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0 contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) # calculate jacobian Jsys, dJsys = controlModel.computeCom_J_dJdq() J_contacts = [] # type: list[np.ndarray] dJ_contacts = [] # type: list[np.ndarray] for contact_id in contact_ids: J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :]) dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6]) # calculate footCenter footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \ else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR)) footCenter[1] = 0. # if len(contact_body_pos) > 2: # hull = ConvexHull(contact_body_pos) footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \ else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR)) footCenter_ref = footCenter_ref + contMotionOffset # if len(ref_body_pos) > 2: # hull = ConvexHull(ref_body_pos) footCenter_ref[1] = 0. # footCenter[0] = footCenter[0] + getParamVal('com X offset') # footCenter[1] = footCenter[0] + getParamVal('com Y offset') # footCenter[2] = footCenter[2] + getParamVal('com Z offset') # initialization if g_initFlag == 0: preFootCenter[0] = footCenter.copy() g_initFlag = 1 # if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01: # contactChangeCount += 30 if contactChangeCount > 0: # change footcenter gradually footCenter = preFootCenter[0] + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter[0]) / maxContactChangeCount else: preFootCenter[0] = footCenter.copy() # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter # CM_ref_plane = footCenter_ref CM_ref = footCenter + np.array([ getParamVal('com X offset'), motionModel.getCOM()[1] + getParamVal('com Y offset'), getParamVal('com Z offset') ]) dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM # dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # print('dCM_plane : ', np.linalg.norm(dCM_plane)) # angular momentum CP_ref = footCenter # CP_ref = footCenter_ref bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP dCP_des = dCP + ddCP_des * (1 / 30.) CP_des = CP + dCP_des * (1 / 30.) # CP_des = footCenter CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount) / maxContactChangeCount else: dH_des = None # convex hull contact_pos_2d = np.asarray([ np.array([contactPosition[0], contactPosition[2]]) for contactPosition in contactPositions ]) p = np.array([CM_plane[0], CM_plane[2]]) # hull = None # type: Delaunay # if contact_pos_2d.shape[0] > 0: # hull = Delaunay(contact_pos_2d) # print(hull.find_simplex(p) >= 0) # set up equality constraint # TODO: # logSO3 is just q'', not acceleration. # To make a_oris acceleration, q'' -> a will be needed # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))])) a_oris = list( map(mm.logSO3, [ np.dot( contact_body_ori[i].T, np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori)) ])) a_oris = list( map(mm.logSO3, [ np.dot( np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY())), contact_body_ori[i].T) for i in range(len(contact_body_ori)) ])) body_qs = list(map(mm.logSO3, contact_body_ori)) body_angs = [ np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori)) ] body_dqs = [ mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs)) ] # a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))] # body_ddq = body_ddqs[0] # body_ori = contact_body_ori[0] # body_ang = np.dot(body_ori.T, contact_body_angvel[0]) # # body_q = mm.logSO3(body_ori) # body_dq = mm.vel2qd(body_ang, body_q) # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q)) KT_SUP = np.diag([kt_sup / 10., kt_sup, kt_sup / 10.]) # KT_SUP = np.diag([kt_sup, kt_sup, kt_sup]) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))])) # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]), # kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))] # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i], # kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))] a_sups = [ np.append( np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i], kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris)) ] # for i in range(len(a_sups)): # a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1] # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### # if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact_left and not contact_right: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 # if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact_right and not contact_left: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) if True: for c_idx in range(len(contact_ids)): mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount = contactChangeCount - 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] ype.nested(ddth_sol_flat, ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) # controlModel.setDOFAccelerations(ddth_des) # controlModel.set_ddq(ddth_sol_flat) # controlModel.set_ddq(ddth_des_flat) controlModel.solveHybridDynamics() if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() controlModel_ik.set_q(controlModel.get_q()) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: control_model_renderer.body_colors[contact_id] = (255, 0, 0) rd_footCenter[0] = footCenter rd_footCenter_ref[0] = footCenter_ref rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) del rd_foot_ori[:] del rd_foot_pos[:] # for seg_foot_id in footIdlist: # rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id)) # rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supL)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supR)) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 400) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] # extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) extraForcePos[0] = controlModel.getBodyPositionGlobal( selectedBody) - 0.1 * np.array([ viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value() ]) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg']) Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot']) Ts['foot_heel_R'] = controlModel.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine']) Ts['head'] = controlModel.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg']) Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel.getJointTransform( idDic['LeftForeArm']) skeleton_renderer.appendFrameState(Ts)
def 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 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()
yme.removeJoint(motion, 'LHipJoint', False) yme.removeJoint(motion, 'RHipJoint', False) yme.removeJoint(motion, 'LowerBack', False) yme.removeJoint(motion, 'LeftToeBase', False) yme.removeJoint(motion, 'RightToeBase', False) yme.removeJoint(motion, 'Neck', False) yme.removeJoint(motion, 'Head', False) yme.removeJoint(motion, 'LeftHandIndex1', False) yme.removeJoint(motion, 'RightHandIndex1', False) yme.removeJoint(motion, 'LeftFingerBase', False) yme.removeJoint(motion, 'RightFingerBase', False) yme.removeJoint(motion, 'LThumb_Effector', False) yme.removeJoint(motion, 'RThumb_Effector', False) yme.removeJoint(motion, 'LThumb', False) yme.removeJoint(motion, 'RThumb', False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(.8, -0.0, -0.4), -.5), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(.8, 0.0, 0.4), -.5), False) motion.updateGlobalT() motion.translateByOffset((0, -0.08, 0)) massMap = {} massMap = massMap.fromkeys([ 'Head', 'Head_Effector', 'Hips', 'LeftArm', 'LeftFoot', 'LeftForeArm', 'LeftHand', 'LeftHand_Effector', 'LeftLeg', 'LeftShoulder', 'LeftToeBase', 'LeftToeBase_Effector', 'LeftUpLeg', 'RightArm', 'RightFoot', 'RightForeArm', 'RightHand', 'RightHand_Effector', 'RightLeg', 'RightShoulder', 'RightToeBase', 'RightToeBase_Effector', 'RightUpLeg', 'Spine', 'Spine1', 'Neck1' ], 0.)
def create_biped(motionName='wd2_n_kick.bvh', SEGMENT_FOOT=True, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=None): """ :param motionName: motion file name :param SEGMENT_FOOT: whether segment foot is :param SEGMENT_FOOT_MAG: :return: """ # :rtype: ym.JointMotion, ypc.ModelConfig, ypc.WorldConfig, int, dict[str, float|dict[str, float]], float if SEGMENT_FOOT_RAD is None: SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5 SEGMENT_FOOT_SEPARATE = False SEGMENT_FOOT_OUTSIDE_JOINT_FIRST = True SEGMENT_FOOT_ARC = True SEGMENT_BETWEEN_SPACE = 1.2 SEGMENT_METATARSAL_LEN = 2.5 SEGMENT_THIRD_PHA_LEN = 1.8 SEGMENT_FOURTH_PHA_RATIO = 5. / 6. SEGMENT_HEEL_LEN = 1.2 # motion # motionName = 'wd2_n_kick.bvh' # motionName = 'wd2_tiptoe.bvh' # motionName = 'wd2_n_kick_zygote.bvh' # motionName = 'wd2_jump.bvh' # motionName = 'wd2_stand.bvh' bvh = yf.readBvhFileAsBvh(motionName) bvh.set_scale(.01) if SEGMENT_FOOT: # partBvhFilePath = '../PyCommon/modules/samples/simpleJump_long_test2.bvh' current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' if SEGMENT_FOOT_SEPARATE: partBvhFilePath = partBvhFilePath + 'simpleJump_long_test5.bvh' elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST: # partBvhFilePath = partBvhFilePath + 'simpleJump_long_test3.bvh' # partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh' partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh' else: partBvhFilePath = partBvhFilePath + 'simpleJump_long_test4.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) SEGMENT_BETWEEN_SPACE = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_1'))[0] SEGMENT_METATARSAL_LEN = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_1_0'))[2] SEGMENT_THIRD_PHA_LEN = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_0_0_Effector'))[2] SEGMENT_HEEL_LEN = abs( partSkeleton.getOffset( partSkeleton.getElementIndex('foot_1_0_Effector'))[2]) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) motion = bvh.toJointMotion(1., False) # type: ym.JointMotion # motion.translateByOffset((0., 0.15, 0.)) # motion.translateByOffset((0., -0.12, 0.)) # motion.rotateByOffset(mm.rotZ(math.pi*1./18.)) # motion = yf.readBvhFile(motionName, .01) # yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False) # yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False) # yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.1), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.1), False) # yme.removeJoint(motion, 'RightFoot_foot_1_1') # yme.removeJoint(motion, 'RightFoot_foot_1_2') # yme.removeJoint(motion, 'LeftFoot_foot_1_1') # yme.removeJoint(motion, 'LeftFoot_foot_1_2') if motionName == 'wd2_n_kick.bvh' or motionName == 'wd2_n_kick_zygote.bvh': yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.04, 0)) for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) motion.extend([motion[-1]] * 300) elif motionName == 'wd2_tiptoe.bvh' or motionName == 'wd2_tiptoe_zygote.bvh': yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1., 0., 0.), -.1), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1., 0., 0.), -.1), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.06, 0)) # if motionName == 'wd2_tiptoe.bvh': # motion.translateByOffset((0, 0.06, 0)) # else: # motion.translateByOffset((0, -0.03, 0)) del motion[:270] for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 # stepsPerFrame = 30 frame_rate = 30 wcfg.timeStep = 1. / (frame_rate * stepsPerFrame) # wcfg.timeStep = (1/30.)/(stepsPerFrame) # wcfg.timeStep = (1/1000.) # width : x axis on body frame # height: y axis on body frame # length: z axis on body frame node = mcfg.getNode('Hips') node.length = 4. / 27. node.width = .25 # node.height = .2 # node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 #node.length = .2 #node.width = .15 node.width = .2 node.mass = 2. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 #node.width = .15 node.width = .2 node.mass = 2. def capsulize(node_name): node_capsule = mcfg.getNode(node_name) node_capsule.geom = 'MyFoot4' node_capsule.width = 0.01 node_capsule.density = 200. # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1)) # node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1)) # capsulize('RightFoot') # capsulize('LeftFoot') if SEGMENT_FOOT: node = mcfg.getNode('RightFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 node.jointType = 'B' node = mcfg.getNode('LeftFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 node.jointType = 'B' # bird foot # capsulize('RightFoot_foot_0_0') # capsulize('RightFoot_foot_0_1') # capsulize('RightFoot_foot_1_0') # capsulize('RightFoot_foot_1_1') # capsulize('RightFoot_foot_2_0') # capsulize('RightFoot_foot_2_1') # capsulize('LeftFoot_foot_0_0') # capsulize('LeftFoot_foot_0_1') # capsulize('LeftFoot_foot_1_0') # capsulize('LeftFoot_foot_1_1') # capsulize('LeftFoot_foot_2_0') # capsulize('LeftFoot_foot_2_1') # human foot if SEGMENT_FOOT: footJointType = 'B' capsulDensity = 400. if SEGMENT_FOOT_SEPARATE: # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') # node.addGeom('MyFoot3', [SEGMENT_FOOT_MAG*np.array([0., 0., 2.5*0.25]), mm.exp([0., 0., 0.])], node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0. - 1.2, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0. + 1.2, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and not SEGMENT_FOOT_ARC: # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' # left foot # outside metatarsals capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and SEGMENT_FOOT_ARC: FIRST_METATARSAL_ANGLE = mm.deg2Rad(30.) SECOND_METATARSAL_ANGLE = mm.deg2Rad(20.) THIRD_METATARSAL_ANGLE = mm.deg2Rad(15.) # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.bone_dir_child = 'RightFoot_foot_0_0_0' # third node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(THIRD_METATARSAL_ANGLE), 0. ]), mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) # fourth node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.bone_dir_child = 'LeftFoot_foot_0_0_0' node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(THIRD_METATARSAL_ANGLE), 0. ]), mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType node.jointType = 'B' # RightFoot_foot_0_0_0 : outside phalanges SEGMENT_FOURTH_PHA_OFFSET = .5 * SEGMENT_THIRD_PHA_LEN * ( 1. - SEGMENT_FOURTH_PHA_RATIO) capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') # third node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # fourth node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([ -SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET ]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN * SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([ SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET ]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN * SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') # second node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(SECOND_METATARSAL_ANGLE), 0. ]), mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) # first node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN * math.tan(FIRST_METATARSAL_ANGLE), 0. ]), mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(SECOND_METATARSAL_ANGLE), 0. ]), mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ -SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN * math.tan(FIRST_METATARSAL_ANGLE), 0. ]), mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., .0]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., .0]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' else: # SEGMENT_FOOT_INSIDE_FIRST # TODO: # adjust transformation of geometries # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0., math.atan2(1.2, 2.5), 0.])], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 15000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} config['weightMap'] = { 'RightArm': .2, 'RightForeArm': .2, 'LeftArm': .2, 'LeftForeArm': .2, 'Spine': .6, 'Spine1': .6, 'RightFoot': .2, 'LeftFoot': .2, 'Hips': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3 } if SEGMENT_FOOT: segfoot_weight = 10. # segfoot_weight = .1 config['weightMap'] = { 'RightArm': .2, 'RightForeArm': .2, 'LeftArm': .2, 'LeftForeArm': .2, 'Spine': .6, 'Spine1': .6, 'RightFoot': .2, 'LeftFoot': .2, 'Hips': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3, 'RightFoot_foot_0_0': segfoot_weight, 'RightFoot_foot_0_1': segfoot_weight, 'RightFoot_foot_1_0': segfoot_weight, 'RightFoot_foot_1_1': segfoot_weight, 'RightFoot_foot_1_2': segfoot_weight, 'RightFoot_foot_0_0_0': segfoot_weight, 'RightFoot_foot_0_1_0': segfoot_weight, 'LeftFoot_foot_0_0': segfoot_weight, 'LeftFoot_foot_0_1': segfoot_weight, 'LeftFoot_foot_1_0': segfoot_weight, 'LeftFoot_foot_1_1': segfoot_weight, 'LeftFoot_foot_1_2': segfoot_weight, 'LeftFoot_foot_0_0_0': segfoot_weight, 'LeftFoot_foot_0_1_0': segfoot_weight } #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5} #success!! ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} ''' #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config, frame_rate
def simulateCallback(frame): global COLOR_ON # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if False: if frame == 200: if motionFile == 'wd2_tiptoe.bvh': setParamVal('tiptoe angle', 0.3) if motionFile == 'wd2_tiptoe_zygote.bvh': setParamVal('tiptoe angle', 0.3) # elif 210 < frame < 240: # if motionFile == 'wd2_tiptoe_zygote.bvh': # setParamVal('com Y offset', 0.01/30. * (frame-110)) elif frame == 400: setParamVal('com Y offset', 0.) setParamVal('tiptoe angle', 0.) elif frame == 430: foot_viewer.check_all_seg() # setParamVal('SupKt', 30.) # elif frame == 400: # setParamVal('SupKt', 17.) # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel.update(motion[frame]) # controlModel_ik.set_q(controlModel.get_q()) # controlModel_ik.set_q(controlModel.get_q()) controlModel_ik.update(motion[frame]) controlModel_ik.translateByOffset( np.array([ -getParamVal('com X offset') * 2., getParamVal('com Y offset'), getParamVal('com Z offset') ])) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list( map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list( map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [ motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_joint_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_ori = list( map(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_vel = [ ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel)) ] is_contact = [1] * len(contact_ids) contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0 contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: motion_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: motion_model_renderer.body_colors[contact_id] = (255, 0, 0) pallete2 = list() pallete2.append((244, 198, 61)) pallete2.append((4, 105, 113)) pallete2.append((234, 219, 196)) pallete2.append((216, 1, 6)) pallete2.append((230, 230, 230)) pallete = pallete2 color = dict() color['RightFoot'] = pallete[0] color['RightFoot_foot_1_0'] = pallete[4] color['RightFoot_foot_0_0'] = pallete[1] color['RightFoot_foot_0_0_0'] = pallete[2] color['RightFoot_foot_0_1_0'] = pallete[3] color['LeftFoot'] = pallete[0] color['LeftFoot_foot_1_0'] = pallete[4] color['LeftFoot_foot_0_0'] = pallete[1] color['LeftFoot_foot_0_0_0'] = pallete[2] color['LeftFoot_foot_0_1_0'] = pallete[3] if COLOR_ON: for color_key in color.keys(): motion_model_renderer.body_colors[ idDic[color_key]] = color[color_key] rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. del rd_foot_ori[:] del rd_foot_pos[:] # for seg_foot_id in footIdlist: # rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id)) # rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supL)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supR)) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel_ik.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel_ik.getJointTransform( idDic['RightUpLeg']) Ts['shin_R'] = controlModel_ik.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel_ik.getJointTransform( idDic['RightFoot']) Ts['foot_heel_R'] = controlModel_ik.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel_ik.getJointTransform( idDic['Spine']) Ts['head'] = controlModel_ik.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel_ik.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel_ik.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel_ik.getJointTransform( idDic['LeftUpLeg']) Ts['shin_L'] = controlModel_ik.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel_ik.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel_ik.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel_ik.getJointTransform( idDic['LeftForeArm']) color = dict() color['foot_R'] = pallete[0] color['heel_R'] = pallete[4] color['outside_metatarsal_R'] = pallete[1] color['outside_phalanges_R'] = pallete[2] color['inside_phalanges_R'] = pallete[3] color['foot_L'] = pallete[0] color['heel_L'] = pallete[4] color['outside_metatarsal_L'] = pallete[1] color['outside_phalanges_L'] = pallete[2] color['inside_phalanges_L'] = pallete[3] if COLOR_ON: skeleton_renderer.appendFrameState(Ts, color) else: skeleton_renderer.appendFrameState(Ts)
def buildMcfg(): massMap = buildMassMap() mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 totalMass = 0. for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] # totalMass += node.mass node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 # node.length = .27 # node.offset = (0,0,0.01) node.width = .1 node.geom = 'MyFoot1' node = mcfg.getNode('LeftFoot') node.length = .25 # node.length = .27 # node.offset = (0,0,0.01) node.width = .1 node.geom = 'MyFoot1' def capsulize(node_name): node = mcfg.getNode(node_name) node.geom = 'MyFoot4' node.width = 0.01 # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1)) # node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1)) # capsulize('RightFoot') # capsulize('LeftFoot') ''' node = mcfg.getNode('RightFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 # node.jointType = 'U' node = mcfg.getNode('LeftFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 # node.jointType = 'U' ''' # bird foot # capsulize('RightFoot_foot_0_0') # capsulize('RightFoot_foot_0_1') # capsulize('RightFoot_foot_1_0') # capsulize('RightFoot_foot_1_1') # capsulize('RightFoot_foot_2_0') # capsulize('RightFoot_foot_2_1') # capsulize('LeftFoot_foot_0_0') # capsulize('LeftFoot_foot_0_1') # capsulize('LeftFoot_foot_1_0') # capsulize('LeftFoot_foot_1_1') # capsulize('LeftFoot_foot_2_0') # capsulize('LeftFoot_foot_2_1') # human foot capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom('MyFoot3', [ 0.02 * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02)) node.addGeom('MyFoot3', [ 0.02 * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02)) # node.addGeom('MyFoot4', [0.02*np.array([-1.2, 0., 0.]), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1)) node.jointType = 'R' capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom('MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.addGeom('MyFoot4', [0.02 * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom('MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.addGeom('MyFoot3', [0.02 * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom('MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.addGeom('MyFoot4', [0.02 * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom('MyFoot3', [0.02 * np.array([0., 0., .7]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, 0.02 * 2.0 + 0.02)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1)) node.jointType = 'R' capsulize('RightFoot_foot_1_1') node = mcfg.getNode('RightFoot_foot_1_1') node.addGeom('MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('RightFoot_foot_1_2') node = mcfg.getNode('RightFoot_foot_1_2') node.addGeom('MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom('MyFoot3', [ 0.02 * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02)) node.addGeom('MyFoot3', [ 0.02 * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom('MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.addGeom('MyFoot4', [0.02 * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom('MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.addGeom('MyFoot3', [0.02 * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom('MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.addGeom('MyFoot4', [0.02 * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom('MyFoot3', [0.02 * np.array([0., 0., .7]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, 0.02 * 2.0 + 0.02)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_1_1') node = mcfg.getNode('LeftFoot_foot_1_1') node.addGeom('MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' capsulize('LeftFoot_foot_1_2') node = mcfg.getNode('LeftFoot_foot_1_2') node.addGeom('MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1)) node.jointType = 'R' return mcfg