#----------------------------------------------------------------------- # # Primitive tsxPython sample script # Simple bipedal walking # for ts4, tS5, tS6 # # $Id: skel2c.py 102 2022-04-15 13:17:23Z 3DfromNULL $ #----------------------------------------------------------------------- import ptsxpy as p from math import * if tscode == 43: ptsxgp.msgbox_excl( "Note", "(For tS4.3 users) Please right click \"Record Key\" red circle button to make sure that the AUTO RECORD is off before you execute this script. You will get glitchy result under the auto record mode." ) # base position bx = 0. by = 0. bz = 4. # definition of constances pi_half = pi / 2. x_axis = Vec3f( 1., 0., 0. ) y_axis = Vec3f( 0., 1., 0. ) z_axis = Vec3f( 0., 0., 1. ) minus_x_axis = Vec3f( -1., 0., 0. ) pskel1wk = ptsxgp.alloc_long( 1 ) pjoint1wk = ptsxgp.alloc_long( 1 ) pjoint2wk = ptsxgp.alloc_long( 1 ) pjoint3wk = ptsxgp.alloc_long( 1 ) pBone1wk = ptsxgp.alloc_long( 1 ) pBone2wk = ptsxgp.alloc_long( 1 ) pBone3wk = ptsxgp.alloc_long( 1 ) pBone4wk = ptsxgp.alloc_long( 1 ) pBone5wk = ptsxgp.alloc_long( 1 ) pBone6wk = ptsxgp.alloc_long( 1 ) pBone7wk = ptsxgp.alloc_long( 1 ) # Make a branch on the bone and make three linked bones. def branch1( skel, bone, dx, dz ): print( "dx, dz=(", dx, ",", dz, ")" ) vec1 = Vec3f() vec2 = Vec3f() vec3 = Vec3f() jtob1 = Vec3f() jtob2 = Vec3f() # Add a bone (as a pelvis) to the lowest bone of the vertebral # column loc3 = Vec3f( bx, by, bz + dz ) p.SkeletonAddBranch( skel, pjoint3wk, e_tsxJOINT_FIXED, loc3.p, bone, pBone4wk ) # Move it to right or left boneA = ptsxgp.loadptr( pBone4wk ) bones.append( boneA ) p.GNodeGetLocation( boneA, vec1.p ) print( "(1) vec1=", vec1.prt() ) #vec1.set_x( vec1.x() + 0.5 * dx ) #vec1.set_z( bz + dz + -.25 ) brloc1 = Vec3f( vec1.x() + 0.5 * dx, 0., bz + dz + -.25 ) p.GNodeSetLocation( boneA, brloc1.p ) p.GNodeGetLocation( boneA, brloc1.p ) print( "(2) brloc1=", brloc1.prt() ) # Add a bone (for a thigh or a humerus) to it loc3 = Vec3f( bx + dx, by, bz + dz - 0.25 ) p.SkeletonAddBranch( skel, pjoint3wk, e_tsxJOINT_HINGE, loc3.p, boneA, pBone5wk ) # Move it down boneB = ptsxgp.loadptr( pBone5wk ) bones.append( boneB ) vec1 = Vec3f() p.GNodeGetLocation( boneB, vec1.p ) print( "(3) vec1=", vec1.prt() ) print( "i=", i ) #vec1.set_x( bx + i ) #vec1.set_x( bx + dx ) #vec1.set_z( bz + dz - 1. ) brloc2 = Vec3f( bx + dx, 0., bz + dz - 1. ) p.GNodeSetLocation( boneB, brloc2.p ) p.GNodeGetLocation( boneB, brloc2.p ) print( "(4) brloc2=", brloc2.prt() ) if tscode == 43 or \ tscode == 60 or \ tscode == 66: # Add a bone (for a shin or a forearm) to it loc3 = Vec3f( bx + dx, by, bz + dz - 0.25 - 1. ) print( "loc3=", loc3.prt() ) p.SkeletonAddBranch( skel, pjoint2wk, e_tsxJOINT_HINGE, loc3.p, boneB, pBone5wk ) #p.SkeletonAddBranch( skel, pjoint2wk, e_tsxJOINT_CUSTOM, loc3.p, boneB, pBone5wk ) boneB = ptsxgp.loadptr( pBone5wk ) bones.append( boneB ) # Point the last joint joint1 = ptsxgp.loadptr( pjoint3wk ) # Exchange the linkage order of two bones associated with the joint # so that we can rightly control the rotation for the leg/arm p.JointGetBones( joint1, pBone6wk, pBone7wk ) boneC = ptsxgp.loadptr( pBone6wk ) boneD = ptsxgp.loadptr( pBone7wk ) print( "(1) boneC=0x%08x" % boneC, ", boneD=0x%08x" % boneD ) #if tscode == 51 or tscode == 43: if tscode == 51: p.JointLinkBones( joint1, boneD, boneC ) p.JointGetBones( joint1, pBone1wk, pBone2wk ) boneV = ptsxgp.loadptr( pBone1wk ) boneW = ptsxgp.loadptr( pBone2wk ) print( "(2) boneV=0x%08x" % boneV, ", boneW=0x%08x" % boneW ) p.GNodeGetLocation( boneV, vec1.p ) print( "boneV loc=", vec1.prt() ) # exchange the directions from the joint to two bones. p.JointGetConnectors( joint1, jtob1.p, jtob2.p ) print( "joint connector1=", jtob1.prt() ) print( "joint connector2=", jtob2.prt() ) p.JointSetConnectors( joint1, jtob2.p, jtob1.p ) # Allow rotation around axes dof1 = p.JointGetDoF( joint1 ) print( "dof1=", dof1 ) #dof1 = p.JointSetDoF( joint1, tsxJOINT_DOF_YAW ) dof1 = p.JointSetDoF( joint1, tsxJOINT_DOF_PITCH + tsxJOINT_DOF_YAW ) dof1 = p.JointGetDoF( joint1 ) print( "dof1=", dof1 ) # Set axis of the rotation to the direction of the world x-axis # so that the leg/arm rotates around world x-axis) p.JointGetAxis( joint1, tsxJOINT_DOF_PITCH, vec3.p ) print( "(5.1) PITCH axis=", vec3.prt() ) if tscode == 43: vec2 = Vec3f( 0., -1., 0. ) p.JointSetAxis( joint1, tsxJOINT_DOF_PITCH, vec2.p ) p.JointGetAxis( joint1, tsxJOINT_DOF_PITCH, vec3.p ) print( "(5.2) PITCH axis=", vec3.prt() ) p.JointGetAxis( joint1, tsxJOINT_DOF_YAW, vec3.p ) print( "(6.1) YAW axis=", vec3.prt() ) if tscode == 43: vec2 = Vec3f( 1., 0., 0. ) p.JointSetAxis( joint1, tsxJOINT_DOF_YAW, vec2.p ) p.JointGetAxis( joint1, tsxJOINT_DOF_YAW, vec3.p ) print( "(6.2) YAW axis=", vec3.prt() ) p.JointSetMinLimit( joint1, tsxJOINT_DOF_TRANS_X, 0. ) p.JointSetMaxLimit( joint1, tsxJOINT_DOF_TRANS_X, 0. ) p.JointSetMinLimit( joint1, tsxJOINT_DOF_TRANS_Y, 0. ) p.JointSetMaxLimit( joint1, tsxJOINT_DOF_TRANS_Y, 0. ) p.JointSetMinLimit( joint1, tsxJOINT_DOF_TRANS_Z, 0. ) p.JointSetMaxLimit( joint1, tsxJOINT_DOF_TRANS_Z, 0. ) #p.JointSetValue( joint1, tsxJOINT_DOF_YAW, 0.) p.JointSetMinLimit( joint1, tsxJOINT_DOF_PITCH, -37 * pi / 180. ) p.JointSetMaxLimit( joint1, tsxJOINT_DOF_PITCH, 24 * pi / 180. ) p.JointSetStiffness( joint1, tsxJOINT_DOF_PITCH, 0. ) if tscode == 43: p.JointSetValue( joint1, tsxJOINT_DOF_PITCH, 0. ) else: p.JointSetValue( joint1, tsxJOINT_DOF_PITCH, -32. ) p.JointSetMinLimit( joint1, tsxJOINT_DOF_YAW, -50 * pi / 180. ) p.JointSetMaxLimit( joint1, tsxJOINT_DOF_YAW, 107 * pi / 180. ) p.JointSetStiffness( joint1, tsxJOINT_DOF_YAW, 0. ) p.JointSetValue( joint1, tsxJOINT_DOF_YAW, 0. ) p.JointSetMinLimit( joint1, tsxJOINT_DOF_ROLL, 0. ) p.JointSetMaxLimit( joint1, tsxJOINT_DOF_ROLL, 0. ) p.JointSetStiffness( joint1, tsxJOINT_DOF_ROLL, 0. ) p.JointSetValue( joint1, tsxJOINT_DOF_ROLL, 0. ) #if tscode == 51 or tscode == 43: if tscode == 51: # Add a bone (for a shin or a forearm) to it loc3 = Vec3f( bx + dx, by, bz + dz - 0.25 - 1. ) print( "loc3=", loc3.prt() ) p.SkeletonAddBranch( skel, pjoint2wk, e_tsxJOINT_HINGE, loc3.p, boneB, pBone5wk ) boneB = ptsxgp.loadptr( pBone5wk ) bones.append( boneB ) return joint1 # def pull1( bone, tup ): #p.SelectSobj( bone, e_tsxSELECT, e_tsxFALSE ) #stop ############ ofst1 = Vec3f() #m = 1. if tscode == 51 else 1. m = 0.5 if tscode == 43 else 1. move_vec = Vec3f( tup[ 0 ], m * tup[ 1 ], tup[ 2 ] ) #p.GNodeGetLocation( bone, ofst1.p ) #print( "ofst1=", ofst1.prt() ) p.IKgroupTranslateEffector( \ skel1, bone, ofst1.p, move_vec.p, e_tsxWorldFrame ) #p.GNodeDraw( skin1 ) # Create a keyframe def create_kf1( tm, skel1, skin1 ): print( "tm=", tm ) if tm >= 0.: p.AnimSetActiveTime( tm ) if tscode == 51: p.SobjSetFrame( skel1, e_tsxKFT_MOVE ) p.SobjSetFrame( skin1, e_tsxKFT_MOVE ) elif tscode == 66: p.SobjSetFrame( skel1, e_tsxKFT_MOVE ) # Take two steps forward def walk1( tm, dtm, vec1_p ): if tscode == 43: p.SobjSetFrame( skel1, e_tsxKFT_MOVE ) p.SobjSetFrame( skin1, e_tsxKFT_MOVE ) p.SobjSetFrame( skel1, e_tsxKFT_KINEMATICS ) p.SobjSetFrame( skin1, e_tsxKFT_KINEMATICS ) # Pull each bone and move body for i in [ -1, 1 ]: if tscode == 43: pull1( bones[ 4 ], ( 0.0, 0.0, -3.0 ) ) # left leg H pull1( bones[ 4 ], ( 0.0, i * -10.5, 0.2 ) ) # left leg H else: pull1( bones[ 4 ], ( 0.0, 0.0, -1.5 ) ) # left leg H pull1( bones[ 4 ], ( 0.0, i * -1.2, 0.2 ) ) # left leg H if tscode == 43: pull1( bones[ 10 ], ( 0.0, i * 10.0, -5.0 ) ) # right leg H pull1( bones[ 10 ], ( 0.0, i * 5.0, 0.2 ) ) # right leg H else: pull1( bones[ 10 ], ( 0.0, 0.0, -1.5 ) ) # right leg H pull1( bones[ 10 ], ( 0.0, i * 1.2, 0.2 ) ) # right leg H if tscode == 43: pull1( bones[ 7 ], ( 0.0, 0.0, -1.0 ) ) # left arm H pull1( bones[ 7 ], ( 0.0, i * 1.0, 0.2 ) ) # left arm H else: pull1( bones[ 7 ], ( 0.0, 0.0, -1.0 ) ) # left arm H pull1( bones[ 7 ], ( 0.0, i * 1.0, 0.2 ) ) # left arm H if tscode == 43: pull1( bones[ 13 ], ( 0.0, 0.0, -2.0 ) ) # right arm H pull1( bones[ 13 ], ( 0.0, i * -1.0, 0.2 ) ) # right arm H else: pull1( bones[ 13 ], ( 0.0, 0.0, -1.0 ) ) # right arm H pull1( bones[ 13 ], ( 0.0, i * -1.0, 0.2 ) ) # right arm H print( "b4=0x%x" % bones[ 4 ] ) print( "b7=0x%x" % bones[ 7 ] ) #vec1 = Vec3f() #p.GNodeGetLocation( bones[ 7 ], vec1.p ) #print( "bone7 pos=", vec1.prt() ) #p.GNodeGetLocation( bones[ 4 ], vec1.p ) #print( "bone4 pos=", vec1.prt() ) p.GNodeTranslate( skel1, vec1_p, e_tsxWorldFrame ) p.GNodeTranslate( skin1, vec1_p, e_tsxWorldFrame ) # Create a keyframe tm = tm + dtm if tscode == 43: p.SobjSetFrame( skel1, e_tsxKFT_MOVE ) p.SobjSetFrame( skin1, e_tsxKFT_MOVE ) p.SobjSetFrame( skel1, e_tsxKFT_KINEMATICS ) p.SobjSetFrame( skin1, e_tsxKFT_KINEMATICS ) create_kf1( tm, skel1, skin1 ) return tm #-------------------- # Main #-------------------- # bones list bones = [] # time of simulation tm = 0. if tscode == 43: p.AnimSetActiveTime( 0. ) #if tscode == 43: #test # p.AnimSetPlayMode( e_tsxPLAY_SCENE ) # p.AnimPlay() # Incremental quantity for time dtm = 30. # (frames) # Create a skeleton with 2 bones (a base (lower) and a neck (higher)). loc1 = Vec3f( bx, by, bz ) p.SkeletonCreate( pskel1wk, pjoint1wk, e_tsxJOINT_HINGE, loc1.p, pBone1wk, pBone2wk ) skel1 = ptsxgp.loadptr( pskel1wk ) p.SceneAddObject( skel1, 0 ) bone1 = ptsxgp.loadptr( pBone2wk ) bone2 = ptsxgp.loadptr( pBone1wk ) #if tscode == 43 or tscode == 66: #210224 memo: it seems that tS66 does the same as tS51, but this condition 66 is necessary to avoid glitch when an animation added. if tscode == 43 or \ tscode == 60 or \ tscode == 66: #210425 memo: it seems that tS60 and tS66 do the same as tS51, but this condition for 60 or 66 is necessary to avoid glitch when an animation added. p.GNodeRotate( skel1, loc1.p, x_axis.p, pi, e_tsxWorldFrame ) if tscode == 43: p.IKgroupPlaceNail( skel1, bone1, 0 ) else: p.IKgroupPlaceNail( skel1, bone2, 0 ) #p.SelectSobj( bone2, e_tsxSELECT, e_tsxFALSE ) # for debug #p.SceneDraw() # for debug # Point the lower bone as the last bone. if tscode == 43: bone1 = ptsxgp.loadptr( pBone1wk ) bone2 = ptsxgp.loadptr( pBone2wk ) bones.append( bone1 ) bones.append( bone2 ) #bone2 = bone1 # Add a bone with a fixed joint under the 2 linked bones. for i in range( 1 ): loc2 = Vec3f( bx, by, bz - 1. - i ) print( "loc2=", loc2.prt() ) p.SkeletonAddBranch( skel1, pjoint2wk, e_tsxJOINT_FIXED, loc2.p, bone2, pBone3wk ) bone3 = ptsxgp.loadptr( pBone3wk ) bones.append( bone3 ) vec1 = Vec3f() p.GNodeGetLocation( bone1, vec1.p ) print( "bone1 pos=", vec1.prt() ) p.GNodeGetLocation( bone2, vec1.p ) print( "bone2 pos=", vec1.prt() ) p.GNodeGetLocation( bone3, vec1.p ) print( "bone3 pos=", vec1.prt() ) #stop ########## # Add a branch with 3 bones movablejs = [] for i in [ -1, 1 ]: for j in [ ( bone3, 0.6, -2. ), ( bone2, 1., -0.25 ) ]: print( "i=", i, ", j=", j ) j1 = branch1( skel1, j[ 0 ], i * j[ 1 ], j[ 2 ] ) movablejs.append( j1 ) #stop ### #stop #### print( "length of movable joint list=", len( movablejs ) ) #stop ### # In case of tS6.0, just pauses to avoid unexpected joints' moving. # In case of tS6.6, prompts user to do an action in tS to avoid glitchy # joint directions. if tscode >= 60: p.SelectSobj( skel1, e_tsxSELECT, e_tsxFALSE ) p.CurrobjEditTool() if tscode == 60: msg1 = "Click \"OK\" to continue." else: # 66 msg1 = "(1) Click the ground (not any object nor icon) BEFORE close this message box. \n"\ "(2) Pull up this message box using Alt key and a few time of Tab key.\n"\ "(3) Click \"OK\" button to continue." ptsxgp.msgbox_excl( "Note", msg1 ) p.CurrobjEditToolClose() # Create skin for head cb0 = p.CreateCube( 3, 0.6, 0.6, 0.6 ) p.SceneAddObject( cb0, 0 ) loc1 = Vec3f( 0., 0., 4.3 ) p.GNodeSetLocation( cb0, loc1.p ) # Create skin for body l1 = [ \ [ 5, 1.2, 0.5, 2., 0., 0., 2.9 ], \ [ 10, 0.3, 0.5, 1.5, 1., 0., 2.8 ], \ [ 10, 0.3, 0.5, 1.5, -1., 0., 2.8 ], \ [ 10, 0.3, 0.5, 1.5, 0.6, 0., 1.0 ], \ [ 10, 0.3, 0.5, 1.5, -0.6, 0., 1.0 ], \ ] cbs = [] for i in range( len( l1 ) ): cb1 = p.CreateCube( l1[ i ][ 0 ], l1[ i ][ 1 ], l1[ i ][ 2 ], l1[ i ][ 3 ] ) cbs.append( cb1 ) p.SceneAddObject( cb1, 0 ) loc1 = Vec3f( l1[ i ][ 4 ], l1[ i ][ 5 ], l1[ i ][ 6 ] ) p.GNodeSetLocation( cb1, loc1.p ) # Group them p.SelectSobj( cb0, e_tsxSELECT, e_tsxFALSE ) for i in range( len( cbs ) ): skin1 = p.GroupAtCurrobj( cbs[ i ] ) # Attach the skin to the skeleton. p.SkeletonAttachSkin( skel1, skin1 ) # Select the skeleton p.SelectSobj( skel1, e_tsxSELECT, e_tsxFALSE ) print( "num bones=", len( bones ) ) #if tscode == 43: #test # ptsxgp.msgbox_excl( "Note", "test" ) #stop ### # Setup animation path of sheet1 before pulling bone #p.SobjCreateScript( skel1 ) vec1 = Vec3f( bx, by - 5., bz ) p.GNodeSetLocation( skel1, vec1.p ) p.GNodeSetLocation( skin1, vec1.p ) p.AnimSetActiveTime( 0. ) # Create a keyframe create_kf1( -1., skel1, skin1 ) # Create a keyframe tm = tm + dtm / 2 create_kf1( tm, skel1, skin1 ) # if tscode == 43: # 210302 note: This "pulling left leg higher part backward" seems to be # ignored and overwitten(?) by following walk1() on tS4.3. pull1( bones[ 4 ], ( 0.0, -0.2, 0.2 ) ) # left leg H pull1( bones[ 5 ], ( 0.0, -5.3, 0.5 ) ) # left leg L pull1( bones[ 4 ], ( 0.0, -0.3, 0.2 ) ) # left leg H pull1( bones[ 10 ], ( 0.0, 1.5, 0.8 ) ) # right leg H pull1( bones[ 11 ], ( 0.0, -0.5, -1.0 ) ) # right leg L pull1( bones[ 7 ], ( 0.0, 0.2, 0.8 ) ) # left arm H pull1( bones[ 8 ], ( 0.0, 0.3, 1.0 ) ) # left arm L pull1( bones[ 13 ], ( 0.0, -0.4, 0.4 ) ) # right arm H pull1( bones[ 14 ], ( 0.0, 0.2, -0.2 ) ) # right arm L else: pull1( bones[ 4 ], ( 0.0, -0.2, 0.2 ) ) # left leg H pull1( bones[ 5 ], ( 0.0, -0.3, 0.2 ) ) # left leg L pull1( bones[ 4 ], ( 0.0, -0.3, 0.2 ) ) # left leg H pull1( bones[ 10 ], ( 0.0, 1.5, 0.8 ) ) # right leg H pull1( bones[ 11 ], ( 0.0, -0.5, -1.0 ) ) # right leg L pull1( bones[ 7 ], ( 0.0, 0.2, 0.8 ) ) # left arm H pull1( bones[ 8 ], ( 0.0, 0.3, 1.0 ) ) # left arm L pull1( bones[ 13 ], ( 0.0, -0.4, 0.4 ) ) # right arm H pull1( bones[ 14 ], ( 0.0, 0.2, -0.2 ) ) # right arm L print( "(1) tm=", tm ) vec1 = Vec3f( 0., 1.0, 0. ) p.GNodeTranslate( skel1, vec1.p, e_tsxWorldFrame ) p.GNodeTranslate( skin1, vec1.p, e_tsxWorldFrame ) # Create a keyframe tm = tm + dtm create_kf1( tm, skel1, skin1 ) # print( "(2) tm=", tm ) vec1 = Vec3f( 0., 1.8, 0. ) tm = walk1( tm, dtm, vec1.p ) tm = walk1( tm, dtm, vec1.p ) tm = walk1( tm, dtm, vec1.p ) tm = walk1( tm, dtm, vec1.p ) p.AnimSetActiveTime( 0. ) p.SceneDraw() p.AnimSetPlayMode( e_tsxPLAY_SCENE ) print( "Push tS \"Play\" button to start the generated animation." )