From bded2968ea8363dac8de89969b650c32f7d25880 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 19 Nov 2018 10:10:33 -0800 Subject: [PATCH 01/10] add PyBullet.rotateVector method add missing walk humanoid3d_walk.txt --- examples/SharedMemory/PhysicsClientC_API.cpp | 10 ++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../pybullet_data/motions/humanoid3d_walk.txt | 45 ++++++++++++++++ examples/pybullet/pybullet.c | 52 +++++++++++++++++++ 4 files changed, 108 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 40eff6ae8..f8ee5b4aa 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -5266,6 +5266,16 @@ B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double outOrn[3] = result[3]; } +B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/]) +{ + b3Quaternion q(quat[0], quat[1], quat[2], quat[3]); + b3Vector3 v = b3MakeVector3(vec[0], vec[1], vec[2]); + b3Vector3 vout = b3QuatRotate(q, v); + vecOut[0] = vout[0]; + vecOut[1] = vout[1]; + vecOut[2] = vout[2]; +} + B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]) { b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 14ce0f706..cb7c40f16 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -639,6 +639,7 @@ extern "C" B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle); B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]); B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]); + B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/]); diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt b/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt new file mode 100644 index 000000000..740688a16 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt @@ -0,0 +1,45 @@ +{ +"Loop": "wrap", +"Frames": +[ +[ 0.0333320000, 0.0000000000, 0.8475320000, 0.0000000000, 0.9986780000, 0.0141040000, -0.0006980000, -0.0494230000, 0.9988130000, 0.0094850000, -0.0475600000, -0.0044750000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9649400000, 0.0243690000, -0.0575550000, 0.2549220000, -0.2491160000, 0.9993660000, 0.0099520000, 0.0326540000, 0.0100980000, 0.9854980000, -0.0644070000, 0.0932430000, -0.1262970000, 0.1705710000, 0.9927550000, -0.0209010000, 0.0888240000, -0.0781780000, -0.3915320000, 0.9828790000, 0.1013910000, -0.0551600000, 0.1436190000, 0.9659420000, 0.1884590000, -0.1422460000, 0.1058540000, 0.5813480000], +[ 0.0333320000, 0.0415400000, 0.8465850000, 0.0047220000, 0.9986070000, -0.0037870000, 0.0074560000, -0.0520870000, 0.9986070000, 0.0231250000, -0.0473580000, 0.0025780000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9670290000, 0.0373040000, -0.0422520000, 0.2483510000, -0.2977710000, 0.9998640000, 0.0127380000, 0.0096330000, -0.0042340000, 0.9855560000, -0.0665800000, 0.0873690000, -0.1288940000, 0.1815050000, 0.9942520000, 0.0102260000, 0.0881860000, -0.0598400000, -0.5307910000, 0.9843850000, 0.0977870000, -0.0740410000, 0.1262580000, 0.9693550000, 0.1748130000, -0.1459730000, 0.0921060000, 0.5672590000], +[ 0.0333320000, 0.0817250000, 0.8482530000, 0.0105010000, 0.9983820000, -0.0253180000, 0.0032440000, -0.0508180000, 0.9981930000, 0.0412880000, -0.0434720000, 0.0040260000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9673350000, 0.0621840000, -0.0445370000, 0.2416880000, -0.3727180000, 0.9994770000, -0.0213330000, 0.0181200000, 0.0161820000, 0.9851630000, -0.0713220000, 0.1006090000, -0.1193520000, 0.1756110000, 0.9943800000, 0.0459520000, 0.0909190000, -0.0288200000, -0.7122180000, 0.9896690000, 0.0717890000, -0.0983600000, 0.0756710000, 0.9762440000, 0.1541960000, -0.1302060000, 0.0788530000, 0.5370070000], +[ 0.0333320000, 0.1169630000, 0.8506510000, 0.0135860000, 0.9981250000, -0.0353450000, 0.0015130000, -0.0499400000, 0.9977950000, 0.0508470000, -0.0416980000, 0.0089820000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9703800000, 0.0745730000, -0.0429560000, 0.2257350000, -0.3977670000, 0.9984780000, -0.0369530000, 0.0142340000, 0.0383920000, 0.9847180000, -0.0777390000, 0.1190810000, -0.1005320000, 0.1713110000, 0.9949900000, 0.0647560000, 0.0743250000, 0.0166370000, -0.9223910000, 0.9933260000, 0.0336230000, -0.1079000000, 0.0230540000, 0.9827350000, 0.1368340000, -0.1094470000, 0.0594120000, 0.5121800000], +[ 0.0333320000, 0.1503280000, 0.8547200000, 0.0157810000, 0.9980230000, -0.0394800000, 0.0077050000, -0.0483040000, 0.9977330000, 0.0519640000, -0.0418120000, 0.0089230000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9747330000, 0.0790650000, -0.0543960000, 0.2017060000, -0.3934650000, 0.9972590000, -0.0417730000, 0.0177010000, 0.0584460000, 0.9850070000, -0.0833490000, 0.1302310000, -0.0765050000, 0.1796490000, 0.9937930000, 0.0745150000, 0.0477340000, 0.0674090000, -1.1046640000, 0.9928650000, 0.0062250000, -0.1077960000, -0.0505960000, 0.9871540000, 0.1235870000, -0.0915170000, 0.0433300000, 0.4724830000], +[ 0.0333320000, 0.1830650000, 0.8600640000, 0.0173490000, 0.9981580000, -0.0400090000, 0.0126040000, -0.0438360000, 0.9979440000, 0.0506730000, -0.0387370000, 0.0062810000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9799630000, 0.0791850000, -0.0591360000, 0.1729320000, -0.3762820000, 0.9962450000, -0.0438390000, 0.0191110000, 0.0721740000, 0.9853390000, -0.0903550000, 0.1340710000, -0.0544840000, 0.2085050000, 0.9897330000, 0.0805020000, 0.0279590000, 0.1147470000, -1.2360120000, 0.9845650000, -0.0265310000, -0.1003850000, -0.1408900000, 0.9893540000, 0.1141940000, -0.0850200000, 0.0301840000, 0.4168040000], +[ 0.0333320000, 0.2152590000, 0.8651470000, 0.0180420000, 0.9985020000, -0.0367590000, 0.0105990000, -0.0391100000, 0.9984290000, 0.0444100000, -0.0339390000, 0.0040300000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9852450000, 0.0761030000, -0.0563350000, 0.1425730000, -0.3495410000, 0.9953800000, -0.0434500000, 0.0206190000, 0.0831060000, 0.9844050000, -0.0980730000, 0.1420870000, -0.0337490000, 0.2497670000, 0.9845040000, 0.0806090000, 0.0192330000, 0.1545420000, -1.3198790000, 0.9833010000, -0.0173270000, -0.0852970000, -0.1598240000, 0.9909870000, 0.1089630000, -0.0765260000, 0.0147010000, 0.3608970000], +[ 0.0333320000, 0.2470560000, 0.8693730000, 0.0179380000, 0.9989030000, -0.0322850000, 0.0051270000, -0.0335350000, 0.9989250000, 0.0357960000, -0.0294280000, 0.0011110000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9898410000, 0.0716480000, -0.0546010000, 0.1100030000, -0.3161610000, 0.9945200000, -0.0444670000, 0.0253540000, 0.0911600000, 0.9815660000, -0.1079990000, 0.1571830000, -0.0125770000, 0.3013270000, 0.9789940000, 0.0788810000, 0.0172150000, 0.1872250000, -1.3577300000, 0.9891780000, 0.0057330000, -0.0707090000, -0.1284310000, 0.9924060000, 0.1060630000, -0.0621780000, -0.0039700000, 0.3157520000], +[ 0.0333320000, 0.2769800000, 0.8722940000, 0.0172890000, 0.9991290000, -0.0268760000, -0.0023020000, -0.0318330000, 0.9992850000, 0.0284200000, -0.0249180000, 0.0004180000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9928290000, 0.0652220000, -0.0553300000, 0.0835140000, -0.2875030000, 0.9934340000, -0.0442090000, 0.0353110000, 0.0994410000, 0.9765250000, -0.1222710000, 0.1770780000, 0.0095710000, 0.3523340000, 0.9722720000, 0.0757490000, 0.0196330000, 0.2203740000, -1.3511600000, 0.9940540000, 0.0327350000, -0.0606840000, -0.0842780000, 0.9934730000, 0.1033700000, -0.0410110000, -0.0253650000, 0.2805770000], +[ 0.0333320000, 0.3062360000, 0.8738900000, 0.0171740000, 0.9991940000, -0.0233840000, -0.0108290000, -0.0307910000, 0.9995110000, 0.0230890000, -0.0210780000, -0.0005190000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9947400000, 0.0606280000, -0.0587570000, 0.0580050000, -0.2592610000, 0.9921310000, -0.0447990000, 0.0471680000, 0.1069800000, 0.9700480000, -0.1378520000, 0.1973670000, 0.0324100000, 0.4029120000, 0.9655460000, 0.0745680000, 0.0249000000, 0.2480750000, -1.3006270000, 0.9968650000, 0.0607040000, -0.0424000000, -0.0278910000, 0.9935740000, 0.1020610000, -0.0138080000, -0.0469420000, 0.2516170000], +[ 0.0333320000, 0.3342070000, 0.8741440000, 0.0164240000, 0.9991300000, -0.0197040000, -0.0202830000, -0.0306650000, 0.9996800000, 0.0180100000, -0.0177710000, -0.0002450000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9960580000, 0.0549300000, -0.0590820000, 0.0368760000, -0.2401550000, 0.9906490000, -0.0457460000, 0.0565300000, 0.1154430000, 0.9626530000, -0.1537270000, 0.2156710000, 0.0561530000, 0.4451960000, 0.9598350000, 0.0731180000, 0.0338350000, 0.2687470000, -1.1992650000, 0.9976300000, 0.0616240000, -0.0216780000, 0.0216220000, 0.9923620000, 0.1015390000, 0.0171820000, -0.0679090000, 0.2264920000], +[ 0.0333320000, 0.3616910000, 0.8735920000, 0.0151980000, 0.9989560000, -0.0165980000, -0.0273870000, -0.0325760000, 0.9997680000, 0.0143740000, -0.0160460000, -0.0007560000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9968970000, 0.0493380000, -0.0577300000, 0.0206980000, -0.2312330000, 0.9893570000, -0.0444080000, 0.0616240000, 0.1241060000, 0.9557200000, -0.1679910000, 0.2277620000, 0.0806430000, 0.4772300000, 0.9556040000, 0.0704640000, 0.0421420000, 0.2829830000, -1.0487890000, 0.9973700000, 0.0658490000, -0.0045910000, 0.0299310000, 0.9901070000, 0.1009600000, 0.0439320000, -0.0869740000, 0.2013980000], +[ 0.0333320000, 0.3886570000, 0.8721270000, 0.0138460000, 0.9986920000, -0.0155010000, -0.0321950000, -0.0365910000, 0.9998060000, 0.0122360000, -0.0154300000, -0.0001980000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9973380000, 0.0456920000, -0.0563030000, 0.0076880000, -0.2255760000, 0.9880090000, -0.0457620000, 0.0644030000, 0.1326500000, 0.9506770000, -0.1786930000, 0.2314570000, 0.1034830000, 0.5028640000, 0.9530950000, 0.0682600000, 0.0505600000, 0.2905080000, -0.8624410000, 0.9979920000, 0.0621880000, 0.0073170000, 0.0095870000, 0.9873870000, 0.1008070000, 0.0654830000, -0.1030330000, 0.1768320000], +[ 0.0333320000, 0.4162070000, 0.8698920000, 0.0128170000, 0.9982790000, -0.0177930000, -0.0365930000, -0.0422430000, 0.9998090000, 0.0128130000, -0.0147600000, 0.0008960000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9975290000, 0.0453390000, -0.0534220000, -0.0052250000, -0.2157660000, 0.9868960000, -0.0450330000, 0.0659550000, 0.1402090000, 0.9471080000, -0.1865960000, 0.2306250000, 0.1223940000, 0.5269460000, 0.9519170000, 0.0678870000, 0.0651890000, 0.2915410000, -0.6550040000, 0.9984650000, 0.0539830000, 0.0111310000, -0.0055220000, 0.9844990000, 0.1012040000, 0.0846310000, -0.1155700000, 0.1551490000], +[ 0.0333320000, 0.4453160000, 0.8670590000, 0.0119150000, 0.9978440000, -0.0228310000, -0.0404430000, -0.0463760000, 0.9997520000, 0.0168290000, -0.0145230000, 0.0013260000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9972860000, 0.0471740000, -0.0525150000, -0.0209060000, -0.2060700000, 0.9862080000, -0.0410770000, 0.0700370000, 0.1442280000, 0.9437600000, -0.1928680000, 0.2311860000, 0.1366480000, 0.5453590000, 0.9522830000, 0.0672370000, 0.0863160000, 0.2849320000, -0.4426240000, 0.9986390000, 0.0506980000, -0.0056960000, -0.0108540000, 0.9808630000, 0.1016680000, 0.1058650000, -0.1279200000, 0.1464150000], +[ 0.0333320000, 0.4768750000, 0.8640800000, 0.0093330000, 0.9972500000, -0.0259170000, -0.0484160000, -0.0497730000, 0.9997120000, 0.0204270000, -0.0125950000, 0.0009340000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9968350000, 0.0439130000, -0.0556600000, -0.0359640000, -0.2058370000, 0.9849700000, -0.0350340000, 0.0819060000, 0.1479800000, 0.9408130000, -0.1976120000, 0.2337280000, 0.1455710000, 0.5620740000, 0.9539720000, 0.0617530000, 0.1125740000, 0.2710180000, -0.2369080000, 0.9988540000, 0.0337440000, -0.0318460000, -0.0117250000, 0.9767650000, 0.1021930000, 0.1266790000, -0.1394210000, 0.1471610000], +[ 0.0333320000, 0.5103140000, 0.8608720000, 0.0054350000, 0.9967350000, -0.0269650000, -0.0565160000, -0.0509760000, 0.9997290000, 0.0207800000, -0.0103310000, -0.0016980000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9963180000, 0.0367660000, -0.0593190000, -0.0497910000, -0.2214360000, 0.9835200000, -0.0312430000, 0.0927800000, 0.1520030000, 0.9390830000, -0.1992430000, 0.2368990000, 0.1493440000, 0.5758430000, 0.9576770000, 0.0560540000, 0.1220800000, 0.2545750000, -0.0806400000, 0.9988660000, 0.0226260000, -0.0412300000, -0.0074390000, 0.9730470000, 0.1040510000, 0.1439350000, -0.1470930000, 0.1518900000], +[ 0.0333320000, 0.5454110000, 0.8576010000, 0.0010220000, 0.9963850000, -0.0276470000, -0.0627650000, -0.0501190000, 0.9997670000, 0.0193860000, -0.0089680000, -0.0030990000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9958970000, 0.0288280000, -0.0587060000, -0.0625510000, -0.2506660000, 0.9828260000, -0.0305860000, 0.0978030000, 0.1534660000, 0.9377420000, -0.1985960000, 0.2440980000, 0.1470230000, 0.5853580000, 0.9596930000, 0.0545350000, 0.1175900000, 0.2493760000, -0.0592230000, 0.9995230000, -0.0059220000, -0.0294090000, 0.0072780000, 0.9708880000, 0.1070260000, 0.1505500000, -0.1524990000, 0.1588850000], +[ 0.0333320000, 0.5809200000, 0.8549060000, -0.0043250000, 0.9964830000, -0.0246090000, -0.0607910000, -0.0521520000, 0.9998220000, 0.0160670000, -0.0097780000, -0.0011460000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9957560000, 0.0184400000, -0.0587140000, -0.0684330000, -0.2949010000, 0.9833820000, -0.0303580000, 0.0973770000, 0.1501890000, 0.9387990000, -0.1946620000, 0.2480910000, 0.1386140000, 0.5890180000, 0.9586480000, 0.0495910000, 0.1034380000, 0.2604540000, -0.1549670000, 0.9995790000, 0.0047500000, -0.0278210000, -0.0067700000, 0.9702230000, 0.1116970000, 0.1506550000, -0.1532770000, 0.1637470000], +[ 0.0333320000, 0.6192950000, 0.8475320000, -0.0000000000, 0.9986780000, -0.0141040000, 0.0006980000, -0.0494230000, 0.9988130000, -0.0094850000, 0.0475600000, -0.0044750000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9927550000, 0.0209010000, -0.0888240000, -0.0781780000, -0.3915320000, 0.9828790000, -0.1013910000, 0.0551600000, 0.1436190000, 0.9659420000, -0.1884590000, 0.1422460000, 0.1058540000, 0.5813480000, 0.9649400000, -0.0243690000, 0.0575550000, 0.2549220000, -0.2491160000, 0.9993660000, -0.0099520000, -0.0326540000, 0.0100980000, 0.9854980000, 0.0644070000, -0.0932430000, -0.1262970000, 0.1705710000], +[ 0.0333320000, 0.6608350000, 0.8465850000, -0.0047220000, 0.9986070000, 0.0037870000, -0.0074560000, -0.0520870000, 0.9986070000, -0.0231250000, 0.0473580000, 0.0025780000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9942520000, -0.0102260000, -0.0881860000, -0.0598400000, -0.5307910000, 0.9843850000, -0.0977870000, 0.0740410000, 0.1262580000, 0.9693550000, -0.1748130000, 0.1459730000, 0.0921060000, 0.5672590000, 0.9670290000, -0.0373040000, 0.0422520000, 0.2483510000, -0.2977710000, 0.9998640000, -0.0127380000, -0.0096330000, -0.0042340000, 0.9855560000, 0.0665800000, -0.0873690000, -0.1288940000, 0.1815050000], +[ 0.0333320000, 0.7010200000, 0.8482530000, -0.0105010000, 0.9983820000, 0.0253180000, -0.0032440000, -0.0508180000, 0.9981930000, -0.0412880000, 0.0434720000, 0.0040260000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9943800000, -0.0459520000, -0.0909190000, -0.0288200000, -0.7122180000, 0.9896690000, -0.0717890000, 0.0983600000, 0.0756710000, 0.9762440000, -0.1541960000, 0.1302060000, 0.0788530000, 0.5370070000, 0.9673350000, -0.0621840000, 0.0445370000, 0.2416880000, -0.3727180000, 0.9994770000, 0.0213330000, -0.0181200000, 0.0161820000, 0.9851630000, 0.0713220000, -0.1006090000, -0.1193520000, 0.1756110000], +[ 0.0333320000, 0.7362580000, 0.8506510000, -0.0135860000, 0.9981250000, 0.0353450000, -0.0015130000, -0.0499400000, 0.9977950000, -0.0508470000, 0.0416980000, 0.0089820000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9949900000, -0.0647560000, -0.0743250000, 0.0166370000, -0.9223910000, 0.9933260000, -0.0336230000, 0.1079000000, 0.0230540000, 0.9827350000, -0.1368340000, 0.1094470000, 0.0594120000, 0.5121800000, 0.9703800000, -0.0745730000, 0.0429560000, 0.2257350000, -0.3977670000, 0.9984780000, 0.0369530000, -0.0142340000, 0.0383920000, 0.9847180000, 0.0777390000, -0.1190810000, -0.1005320000, 0.1713110000], +[ 0.0333320000, 0.7696230000, 0.8547200000, -0.0157810000, 0.9980230000, 0.0394800000, -0.0077050000, -0.0483040000, 0.9977330000, -0.0519640000, 0.0418120000, 0.0089230000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9937930000, -0.0745150000, -0.0477340000, 0.0674090000, -1.1046640000, 0.9928650000, -0.0062250000, 0.1077960000, -0.0505960000, 0.9871540000, -0.1235870000, 0.0915170000, 0.0433300000, 0.4724830000, 0.9747330000, -0.0790650000, 0.0543960000, 0.2017060000, -0.3934650000, 0.9972590000, 0.0417730000, -0.0177010000, 0.0584460000, 0.9850070000, 0.0833490000, -0.1302310000, -0.0765050000, 0.1796490000], +[ 0.0333320000, 0.8023600000, 0.8600640000, -0.0173490000, 0.9981580000, 0.0400090000, -0.0126040000, -0.0438360000, 0.9979440000, -0.0506730000, 0.0387370000, 0.0062810000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9897330000, -0.0805020000, -0.0279590000, 0.1147470000, -1.2360120000, 0.9845650000, 0.0265310000, 0.1003850000, -0.1408900000, 0.9893540000, -0.1141940000, 0.0850200000, 0.0301840000, 0.4168040000, 0.9799630000, -0.0791850000, 0.0591360000, 0.1729320000, -0.3762820000, 0.9962450000, 0.0438390000, -0.0191110000, 0.0721740000, 0.9853390000, 0.0903550000, -0.1340710000, -0.0544840000, 0.2085050000], +[ 0.0333320000, 0.8345540000, 0.8651470000, -0.0180420000, 0.9985020000, 0.0367590000, -0.0105990000, -0.0391100000, 0.9984290000, -0.0444100000, 0.0339390000, 0.0040300000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9845040000, -0.0806090000, -0.0192330000, 0.1545420000, -1.3198790000, 0.9833010000, 0.0173270000, 0.0852970000, -0.1598240000, 0.9909870000, -0.1089630000, 0.0765260000, 0.0147010000, 0.3608970000, 0.9852450000, -0.0761030000, 0.0563350000, 0.1425730000, -0.3495410000, 0.9953800000, 0.0434500000, -0.0206190000, 0.0831060000, 0.9844050000, 0.0980730000, -0.1420870000, -0.0337490000, 0.2497670000], +[ 0.0333320000, 0.8663510000, 0.8693730000, -0.0179380000, 0.9989030000, 0.0322850000, -0.0051270000, -0.0335350000, 0.9989250000, -0.0357960000, 0.0294280000, 0.0011110000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9789940000, -0.0788810000, -0.0172150000, 0.1872250000, -1.3577300000, 0.9891780000, -0.0057330000, 0.0707090000, -0.1284310000, 0.9924060000, -0.1060630000, 0.0621780000, -0.0039700000, 0.3157520000, 0.9898410000, -0.0716480000, 0.0546010000, 0.1100030000, -0.3161610000, 0.9945200000, 0.0444670000, -0.0253540000, 0.0911600000, 0.9815660000, 0.1079990000, -0.1571830000, -0.0125770000, 0.3013270000], +[ 0.0333320000, 0.8962750000, 0.8722940000, -0.0172890000, 0.9991290000, 0.0268760000, 0.0023020000, -0.0318330000, 0.9992850000, -0.0284200000, 0.0249180000, 0.0004180000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9722720000, -0.0757490000, -0.0196330000, 0.2203740000, -1.3511600000, 0.9940540000, -0.0327350000, 0.0606840000, -0.0842780000, 0.9934730000, -0.1033700000, 0.0410110000, -0.0253650000, 0.2805770000, 0.9928290000, -0.0652220000, 0.0553300000, 0.0835140000, -0.2875030000, 0.9934340000, 0.0442090000, -0.0353110000, 0.0994410000, 0.9765250000, 0.1222710000, -0.1770780000, 0.0095710000, 0.3523340000], +[ 0.0333320000, 0.9255310000, 0.8738900000, -0.0171740000, 0.9991940000, 0.0233840000, 0.0108290000, -0.0307910000, 0.9995110000, -0.0230890000, 0.0210780000, -0.0005190000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9655460000, -0.0745680000, -0.0249000000, 0.2480750000, -1.3006270000, 0.9968650000, -0.0607040000, 0.0424000000, -0.0278910000, 0.9935740000, -0.1020610000, 0.0138080000, -0.0469420000, 0.2516170000, 0.9947400000, -0.0606280000, 0.0587570000, 0.0580050000, -0.2592610000, 0.9921310000, 0.0447990000, -0.0471680000, 0.1069800000, 0.9700480000, 0.1378520000, -0.1973670000, 0.0324100000, 0.4029120000], +[ 0.0333320000, 0.9535020000, 0.8741440000, -0.0164240000, 0.9991300000, 0.0197040000, 0.0202830000, -0.0306650000, 0.9996800000, -0.0180100000, 0.0177710000, -0.0002450000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9598350000, -0.0731180000, -0.0338350000, 0.2687470000, -1.1992650000, 0.9976300000, -0.0616240000, 0.0216780000, 0.0216220000, 0.9923620000, -0.1015390000, -0.0171820000, -0.0679090000, 0.2264920000, 0.9960580000, -0.0549300000, 0.0590820000, 0.0368760000, -0.2401550000, 0.9906490000, 0.0457460000, -0.0565300000, 0.1154430000, 0.9626530000, 0.1537270000, -0.2156710000, 0.0561530000, 0.4451960000], +[ 0.0333320000, 0.9809860000, 0.8735920000, -0.0151980000, 0.9989560000, 0.0165980000, 0.0273870000, -0.0325760000, 0.9997680000, -0.0143740000, 0.0160460000, -0.0007560000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9556040000, -0.0704640000, -0.0421420000, 0.2829830000, -1.0487890000, 0.9973700000, -0.0658490000, 0.0045910000, 0.0299310000, 0.9901070000, -0.1009600000, -0.0439320000, -0.0869740000, 0.2013980000, 0.9968970000, -0.0493380000, 0.0577300000, 0.0206980000, -0.2312330000, 0.9893570000, 0.0444080000, -0.0616240000, 0.1241060000, 0.9557200000, 0.1679910000, -0.2277620000, 0.0806430000, 0.4772300000], +[ 0.0333320000, 1.0079520000, 0.8721270000, -0.0138460000, 0.9986920000, 0.0155010000, 0.0321950000, -0.0365910000, 0.9998060000, -0.0122360000, 0.0154300000, -0.0001980000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9530950000, -0.0682600000, -0.0505600000, 0.2905080000, -0.8624410000, 0.9979920000, -0.0621880000, -0.0073170000, 0.0095870000, 0.9873870000, -0.1008070000, -0.0654830000, -0.1030330000, 0.1768320000, 0.9973380000, -0.0456920000, 0.0563030000, 0.0076880000, -0.2255760000, 0.9880090000, 0.0457620000, -0.0644030000, 0.1326500000, 0.9506770000, 0.1786930000, -0.2314570000, 0.1034830000, 0.5028640000], +[ 0.0333320000, 1.0355020000, 0.8698920000, -0.0128170000, 0.9982790000, 0.0177930000, 0.0365930000, -0.0422430000, 0.9998090000, -0.0128130000, 0.0147600000, 0.0008960000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9519170000, -0.0678870000, -0.0651890000, 0.2915410000, -0.6550040000, 0.9984650000, -0.0539830000, -0.0111310000, -0.0055220000, 0.9844990000, -0.1012040000, -0.0846310000, -0.1155700000, 0.1551490000, 0.9975290000, -0.0453390000, 0.0534220000, -0.0052250000, -0.2157660000, 0.9868960000, 0.0450330000, -0.0659550000, 0.1402090000, 0.9471080000, 0.1865960000, -0.2306250000, 0.1223940000, 0.5269460000], +[ 0.0333320000, 1.0646110000, 0.8670590000, -0.0119150000, 0.9978440000, 0.0228310000, 0.0404430000, -0.0463760000, 0.9997520000, -0.0168290000, 0.0145230000, 0.0013260000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9522830000, -0.0672370000, -0.0863160000, 0.2849320000, -0.4426240000, 0.9986390000, -0.0506980000, 0.0056960000, -0.0108540000, 0.9808630000, -0.1016680000, -0.1058650000, -0.1279200000, 0.1464150000, 0.9972860000, -0.0471740000, 0.0525150000, -0.0209060000, -0.2060700000, 0.9862080000, 0.0410770000, -0.0700370000, 0.1442280000, 0.9437600000, 0.1928680000, -0.2311860000, 0.1366480000, 0.5453590000], +[ 0.0333320000, 1.0961700000, 0.8640800000, -0.0093330000, 0.9972500000, 0.0259170000, 0.0484160000, -0.0497730000, 0.9997120000, -0.0204270000, 0.0125950000, 0.0009340000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9539720000, -0.0617530000, -0.1125740000, 0.2710180000, -0.2369080000, 0.9988540000, -0.0337440000, 0.0318460000, -0.0117250000, 0.9767650000, -0.1021930000, -0.1266790000, -0.1394210000, 0.1471610000, 0.9968350000, -0.0439130000, 0.0556600000, -0.0359640000, -0.2058370000, 0.9849700000, 0.0350340000, -0.0819060000, 0.1479800000, 0.9408130000, 0.1976120000, -0.2337280000, 0.1455710000, 0.5620740000], +[ 0.0333320000, 1.1296090000, 0.8608720000, -0.0054350000, 0.9967350000, 0.0269650000, 0.0565160000, -0.0509760000, 0.9997290000, -0.0207800000, 0.0103310000, -0.0016980000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9576770000, -0.0560540000, -0.1220800000, 0.2545750000, -0.0806400000, 0.9988660000, -0.0226260000, 0.0412300000, -0.0074390000, 0.9730470000, -0.1040510000, -0.1439350000, -0.1470930000, 0.1518900000, 0.9963180000, -0.0367660000, 0.0593190000, -0.0497910000, -0.2214360000, 0.9835200000, 0.0312430000, -0.0927800000, 0.1520030000, 0.9390830000, 0.1992430000, -0.2368990000, 0.1493440000, 0.5758430000], +[ 0.0333320000, 1.1647060000, 0.8576010000, -0.0010220000, 0.9963850000, 0.0276470000, 0.0627650000, -0.0501190000, 0.9997670000, -0.0193860000, 0.0089680000, -0.0030990000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9596930000, -0.0545350000, -0.1175900000, 0.2493760000, -0.0592230000, 0.9995230000, 0.0059220000, 0.0294090000, 0.0072780000, 0.9708880000, -0.1070260000, -0.1505500000, -0.1524990000, 0.1588850000, 0.9958970000, -0.0288280000, 0.0587060000, -0.0625510000, -0.2506660000, 0.9828260000, 0.0305860000, -0.0978030000, 0.1534660000, 0.9377420000, 0.1985960000, -0.2440980000, 0.1470230000, 0.5853580000], +[ 0.0333320000, 1.2002150000, 0.8549060000, 0.0043250000, 0.9964830000, 0.0246090000, 0.0607910000, -0.0521520000, 0.9998220000, -0.0160670000, 0.0097780000, -0.0011460000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9586480000, -0.0495910000, -0.1034380000, 0.2604540000, -0.1549670000, 0.9995790000, -0.0047500000, 0.0278210000, -0.0067700000, 0.9702230000, -0.1116970000, -0.1506550000, -0.1532770000, 0.1637470000, 0.9957560000, -0.0184400000, 0.0587140000, -0.0684330000, -0.2949010000, 0.9833820000, 0.0303580000, -0.0973770000, 0.1501890000, 0.9387990000, 0.1946620000, -0.2480910000, 0.1386140000, 0.5890180000], +[ 0.0000000000, 1.2385900000, 0.8475320000, -0.0000000000, 0.9965700000, 0.0147720000, 0.0589580000, -0.0561480000, 0.9999300000, -0.0073250000, 0.0089320000, 0.0024790000, 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.9606960000, -0.0389450000, -0.0711450000, 0.2654910000, -0.2366380000, 0.9998620000, -0.0068670000, 0.0079280000, -0.0128890000, 0.9703810000, -0.1177290000, -0.1477780000, -0.1505410000, 0.1671120000, 0.9959150000, 0.0009110000, 0.0595720000, -0.0678530000, -0.3567200000, 0.9845480000, 0.0261520000, -0.0998040000, 0.1414910000, 0.9415800000, 0.1883880000, -0.2513170000, 0.1215580000, 0.5897850000] +] +} \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d25f896c4..1086614c3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8808,6 +8808,53 @@ static PyObject* pybullet_invertTransform(PyObject* self, return NULL; } +static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* quatObj; + PyObject* vectorObj; + double quat[4]; + double vec[3]; + int physicsClientId = 0; + int hasQuat = 0; + int hasVec = 0; + + static char* kwlist[] = { "quaternion", "vector", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatObj, &vectorObj, &physicsClientId)) + { + return NULL; + } + + if (quatObj) + { + hasQuat = pybullet_internalSetVector4d(quatObj, quat); + } + + if (vectorObj) + { + hasVec = pybullet_internalSetVectord(vectorObj, vec); + } + if (hasQuat && hasVec) + { + double vecOut[3]; + b3RotateVector(quat, vec, vecOut); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(vecOut[i])); + return pylist; + } + } + else + { + PyErr_SetString(SpamError, "Require quaternion with 4 components [x,y,z,w] and a vector [x,y,z]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds) { @@ -10315,6 +10362,11 @@ static PyMethodDef SpamMethods[] = { { "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS, "Compute the angular velocity given start and end quaternion and delta time." }, + { "rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS, + "Rotate a vector using a quaternion." }, + + + {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS, "Given an object id, joint positions, joint velocities and joint " "accelerations, compute the joint forces using Inverse Dynamics"}, From 41e28e3587adacc14d0cba7b5b4833b4994bf901 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 20 Nov 2018 21:40:20 -0800 Subject: [PATCH 02/10] Add link/joint names for DeepMimic humanoid urdf --- .../gym/pybullet_data/humanoid/humanoid.urdf | 128 +++++++++--------- 1 file changed, 66 insertions(+), 62 deletions(-) diff --git a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf index 9a3d4938e..334b5eda1 100644 --- a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf +++ b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf @@ -1,12 +1,12 @@ - + - + @@ -19,12 +19,12 @@ - - - + + + - + @@ -37,12 +37,12 @@ - - - + + + - + @@ -55,12 +55,12 @@ - - - + + + - + @@ -73,12 +73,12 @@ - - - + + + - + @@ -91,13 +91,14 @@ - - - - + + + + + - + @@ -110,12 +111,12 @@ - - - + + + - + @@ -128,12 +129,12 @@ - - - + + + - + @@ -146,13 +147,14 @@ - - - + + + + - + @@ -165,12 +167,12 @@ - - - + + + - + @@ -183,12 +185,12 @@ - - - + + + - + @@ -201,13 +203,14 @@ - - - + + + + - + @@ -220,12 +223,12 @@ - - - + + + - + @@ -238,12 +241,12 @@ - - - + + + - + @@ -256,13 +259,14 @@ - - - + + + + - + @@ -275,9 +279,9 @@ - - - + + + From f93e4e45536a5090074c58b2d168555e812efa22 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 21 Nov 2018 11:07:05 -0800 Subject: [PATCH 03/10] fix mjcf2urdf.py for Python 3 (utf) and don't export visuals (let it auto-convert from collision element) --- .../gym/pybullet_utils/examples/mjcf2urdf.py | 9 ++++++++- examples/pybullet/gym/pybullet_utils/urdfEditor.py | 12 ++++++------ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py index c1e39745d..f7903262f 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py @@ -18,4 +18,11 @@ for o in objs: humanoid = objs[o] ed0 = ed.UrdfEditor() ed0.initializeFromBulletBody(humanoid, p._client) - ed0.saveUrdf(p.getBodyInfo(0)[1]+"_"+p.getBodyInfo(o)[0]+".urdf") + robotName = str(p.getBodyInfo(o)[1],'utf-8') + partName = str(p.getBodyInfo(o)[0], 'utf-8') + + print("robotName=",robotName) + print("partName=",partName) + + saveVisuals=False + ed0.saveUrdf(robotName+"_"+partName+".urdf", saveVisuals) diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 6b05ce72c..3edca4596 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -273,7 +273,7 @@ class UrdfEditor(object): file.write("\t\t\n") - def writeLink(self, file, urdfLink): + def writeLink(self, file, urdfLink,saveVisuals): file.write("\t\n") @@ -283,9 +283,9 @@ class UrdfEditor(object): for v in urdfLink.urdf_visual_shapes: if (v.geom_type == p.GEOM_CAPSULE): hasCapsules = True - if (not hasCapsules): - for v in urdfLink.urdf_visual_shapes: - self.writeVisualShape(file,v) + if (saveVisuals and not hasCapsules): + for v in urdfLink.urdf_visual_shapes: + self.writeVisualShape(file,v) for c in urdfLink.urdf_collision_shapes: self.writeCollisionShape(file,c) file.write("\t\n") @@ -329,7 +329,7 @@ class UrdfEditor(object): file.write(str) file.write("\t\n") - def saveUrdf(self, fileName): + def saveUrdf(self, fileName, saveVisuals=True): file = open(fileName,"w") file.write("\n") file.write("\n") for link in self.urdfLinks: - self.writeLink(file,link) + self.writeLink(file,link, saveVisuals) for joint in self.urdfJoints: self.writeJoint(file,joint) From 121cdc91b02a80bdb24aa572ba2bbc4f3014b84d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 21 Nov 2018 11:09:10 -0800 Subject: [PATCH 04/10] more work on PyBullet implementation of DeepMimic humanoid mimic of motion capture. b3Quaternion, deal with zero-length axis (in axis,angle constructor) --- .../gym/pybullet_envs/mimic/humanoid.py | 310 +++++++++++++++--- .../gym/pybullet_envs/mimic/humanoid_test.py | 56 ++-- src/Bullet3Common/b3Quaternion.h | 13 +- 3 files changed, 300 insertions(+), 79 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py index 4a5dd01d9..c3b0d0970 100644 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py +++ b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py @@ -4,6 +4,9 @@ currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfram parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) +jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", + "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] + class HumanoidPose(object): def __init__(self): pass @@ -57,9 +60,9 @@ class HumanoidPose(object): def NormalizeQuaternion(self, orn): length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]+orn[3]*orn[3] if (length2>0): - length = math.sqrt(length2) - print("Normalize? length=",length) - + length = math.sqrt(length2) + #print("Normalize? length=",length) + def PostProcessMotionData(self, frameData): baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] @@ -74,7 +77,7 @@ class HumanoidPose(object): leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] - + def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ): keyFrameDuration = frameData[0] basePos1Start = [frameData[1],frameData[2],frameData[3]] @@ -111,7 +114,7 @@ class HumanoidPose(object): rightKneeRotStart = [frameData[20]] rightKneeRotEnd = [frameDataNext[20]] self._rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] - self._rightKneeVel = (rightKneeRotEnd[0]-rightKneeRotStart[0])/keyFrameDuration + self._rightKneeVel = [(rightKneeRotEnd[0]-rightKneeRotStart[0])/keyFrameDuration] rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] @@ -126,7 +129,7 @@ class HumanoidPose(object): rightElbowRotStart = [frameData[29]] rightElbowRotEnd = [frameDataNext[29]] self._rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] - self._rightElbowVel = (rightElbowRotEnd[0]-rightElbowRotStart[0])/keyFrameDuration + self._rightElbowVel = [(rightElbowRotEnd[0]-rightElbowRotStart[0])/keyFrameDuration] leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] @@ -136,7 +139,7 @@ class HumanoidPose(object): leftKneeRotStart = [frameData[34]] leftKneeRotEnd = [frameDataNext[34]] self._leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ] - self._leftKneeVel = (leftKneeRotEnd[0]-leftKneeRotStart[0])/keyFrameDuration + self._leftKneeVel = [(leftKneeRotEnd[0]-leftKneeRotStart[0])/keyFrameDuration] leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] @@ -151,7 +154,7 @@ class HumanoidPose(object): leftElbowRotStart = [frameData[43]] leftElbowRotEnd = [frameDataNext[43]] self._leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] - self._leftElbowVel = (leftElbowRotEnd[0]-leftElbowRotStart[0])/keyFrameDuration + self._leftElbowVel = [(leftElbowRotEnd[0]-leftElbowRotStart[0])/keyFrameDuration] class Humanoid(object): @@ -165,8 +168,12 @@ class Humanoid(object): self._pybullet_client = pybullet_client self._motion_data = motion_data self._humanoid = self._pybullet_client.loadURDF( - "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25) - + "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False) + + #self._humanoidDebug = self._pybullet_client.loadURDF( + # "humanoid/humanoid.urdf", [0,0.9,3],globalScaling=0.25, useFixedBase=True) + + print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid)) pose = HumanoidPose() for i in range (self._motion_data.NumFrames()-1): @@ -176,8 +183,11 @@ class Humanoid(object): self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,self._baseShift,[0,0,0,1]) self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0) for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + ji = self._pybullet_client.getJointInfo(self._humanoid,j) self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0) self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,1]) + print("joint[",j,"].type=",jointTypes[ji[2]]) + print("joint[",j,"].name=",ji[1]) self._initial_state = self._pybullet_client.saveState() self._allowed_body_parts=[11,14] @@ -185,18 +195,41 @@ class Humanoid(object): def Reset(self): self._pybullet_client.restoreState(self._initial_state) - self.SetFrameTime(0) + self.SetSimTime(100) pose = self.InitializePoseFromMotionData() - self.ApplyPose(pose, True) + self.ApplyPose(pose, True, True) + def CalcCycleCount(self, simTime, cycleTime): + phases = simTime / cycleTime; + count = math.floor(phases) + loop = True + #count = (loop) ? count : cMathUtil::Clamp(count, 0, 1); + return count - def SetFrameTime(self, t): - self._frameTime = t - self._frame = int(self._frameTime) + def SetSimTime(self, t): + self._simTime = t + print("SetTimeTime time =",t) + keyFrameDuration = self._motion_data.KeyFrameDuraction() + cycleTime = keyFrameDuration*(self._motion_data.NumFrames()-1) + #print("self._motion_data.NumFrames()=",self._motion_data.NumFrames()) + #print("cycleTime=",cycleTime) + cycles = self.CalcCycleCount(t, cycleTime) + #print("cycles=",cycles) + frameTime = t - cycles*cycleTime + if (frameTime<0): + frameTime += cycleTime + + #print("keyFrameDuration=",keyFrameDuration) + #print("frameTime=",frameTime) + self._frame = int(frameTime/keyFrameDuration) + #print("self._frame=",self._frame) + self._frameNext = self._frame+1 if (self._frameNext >= self._motion_data.NumFrames()): self._frameNext = self._frame - self._frameFraction = self._frameTime - self._frame + + self._frameFraction = (frameTime - self._frame*keyFrameDuration)/(keyFrameDuration) + #print("self._frameFraction=",self._frameFraction) def Terminates(self): #check if any non-allowed body part hits the ground @@ -208,14 +241,50 @@ class Humanoid(object): part=p[3] if (p[2]==self._humanoid): part=p[4] - if (part not in self._allowed_body_parts): + if (part >=0 and part not in self._allowed_body_parts): terminates=True return terminates - - + def BuildHeadingTrans(self, rootOrn): + #align root transform 'forward' with world-space x axis + eul = self._pybullet_client.getEulerFromQuaternion(rootOrn) + refDir = [1,0,0] + rotVec = self._pybullet_client.rotateVector(rootOrn, refDir) + heading = math.atan2(-rotVec[2], rotVec[0]) + heading2=eul[1] + print("heading=",heading) + headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading) + return headingOrn + + def GetPhase(self): + keyFrameDuration = self._motion_data.KeyFrameDuraction() + cycleTime = keyFrameDuration*(self._motion_data.NumFrames()-1) + phase = self._simTime / cycleTime + phase = math.fmod(phase,1.0) + if (phase<0): + phase += 1 + return phase + + def BuildOriginTrans(self): + rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) + print("rootPos=",rootPos, " rootOrn=",rootOrn) + invRootPos=[-rootPos[0], 0, -rootPos[2]] + #invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn) + headingOrn = self.BuildHeadingTrans(rootOrn) + print("headingOrn=",headingOrn) + headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn) + print("headingMat=",headingMat) + #dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn) + #dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn) + + invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms( [0,0,0],headingOrn, invRootPos,[0,0,0,1]) + print("invOrigTransPos=",invOrigTransPos) + print("invOrigTransOrn=",invOrigTransOrn) + invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn) + print("invOrigTransMat =",invOrigTransMat ) + return invOrigTransPos, invOrigTransOrn def InitializePoseFromMotionData(self): frameData = self._motion_data._motion_data['Frames'][self._frame] @@ -225,27 +294,94 @@ class Humanoid(object): return pose + + def ApplyAction(self, action): #turn action into pose pose = HumanoidPose() - #todo: convert action vector into pose - #convert from angle-axis to quaternion for spherical joints + pose.Reset() + index=0 + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._chestRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + #print("pose._chestRot=",pose._chestRot) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._neckRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._rightHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + pose._rightKneeRot = [angle] + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._rightAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._rightShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + pose._rightElbowRot = [angle] + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._leftHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + pose._leftKneeRot = [angle] + + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._leftAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + pose._leftShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + pose._leftElbowRot = [angle] + + + print("index=",index) initializeBase = False - self.ApplyPose(pose, initializeBase) + initializeVelocities = False + self.ApplyPose(pose, initializeBase, initializeVelocities) - def ApplyPose(self, pose, initializeBase): + + def ApplyPose(self, pose, initializeBase, initializeVelocities): + #todo: get tunable parametes from a json file or from URDF (kd, maxForce) if (initializeBase): self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,0,0,1]) basePos=[pose._basePos[0]+self._baseShift[0],pose._basePos[1]+self._baseShift[1],pose._basePos[2]+self._baseShift[2]] + self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, basePos, pose._baseOrn) - self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel) - print("resetBaseVelocity=",pose._baseLinVel) + if initializeVelocities: + self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel) + #print("resetBaseVelocity=",pose._baseLinVel) else: self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1]) - maxForce=1000 - kp=0.8 + + kp=0.01 chest=1 neck=2 rightShoulder=3 @@ -261,29 +397,101 @@ class Humanoid(object): controlMode = self._pybullet_client.POSITION_CONTROL if (initializeBase): - self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot) + if initializeVelocities: + self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot, pose._chestVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot, pose._neckVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot, pose._rightHipVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot, pose._leftHipVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel) + else: + self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=maxForce) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60) + + #debug space + #if (False): + # for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + # js = self._pybullet_client.getJointState(self._humanoid, j) + # self._pybullet_client.resetJointState(self._humanoidDebug, j,js[0]) + # jsm = self._pybullet_client.getJointStateMultiDof(self._humanoid, j) + # if (len(jsm[0])>0): + # self._pybullet_client.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0]) + + def GetState(self): + + stateVector = [] + phase = self.GetPhase() + print("phase=",phase) + stateVector.append(phase) + + rootTransPos, rootTransOrn=self.BuildOriginTrans() + basePos,baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) + + rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1]) + #print("!!!rootPosRel =",rootPosRel ) + #print("rootTransPos=",rootTransPos) + print("basePos=",basePos) + localPos,localOrn = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn , basePos,baseOrn ) + + localPos=[localPos[0]-rootPosRel[0],localPos[1]-rootPosRel[1],localPos[2]-rootPosRel[2]] + print("localPos=",localPos) + + stateVector.append(rootPosRel[1]) + + + for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True) + linkPos = ls[0] + linkOrn = ls[1] + linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, linkPos,linkOrn) + if (linkOrnLocal[3]<0): + linkOrnLocal=[-linkOrnLocal[0],-linkOrnLocal[1],-linkOrnLocal[2],-linkOrnLocal[3]] + linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]] + for l in linkPosLocal: + stateVector.append(l) + for l in linkOrnLocal: + stateVector.append(l) + + + for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True) + linkLinVel = ls[6] + linkAngVel = ls[7] + for l in linkLinVel: + stateVector.append(l) + for l in linkAngVel: + stateVector.append(l) + + print("stateVector len=",len(stateVector)) + return stateVector + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py index 975b2004b..b64ec71ea 100644 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py +++ b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py @@ -3,9 +3,9 @@ currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfram parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) -from pybullet_envs.deep_mimic.humanoid import Humanoid +from pybullet_envs.mimic.humanoid import Humanoid from pybullet_utils.bullet_client import BulletClient -from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData +from pybullet_envs.mimic.motion_capture_data import MotionCaptureData import pybullet_data import pybullet import time @@ -17,17 +17,19 @@ bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP,1) bc.setGravity(0,-9.8,0) motion=MotionCaptureData() -motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"#humanoid3d_spinkick.txt"#humanoid3d_backflip.txt" +motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt" motion.Load(motionPath) print("numFrames = ", motion.NumFrames()) -frameTimeId= bc.addUserDebugParameter("frameTime",0,motion.NumFrames()-1.1,0) +simTimeId= bc.addUserDebugParameter("simTime",0,motion.NumFrames()-1.1,0) y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0]) -bc.loadURDF("plane.urdf",[0,-0.08,0], y2zOrn) +bc.loadURDF("plane.urdf",[0,-0.04,0], y2zOrn) humanoid = Humanoid(bc, motion,[0,0,0]) -frameTime = 0 +simTime = 0 + + keyFrameDuration = motion.KeyFrameDuraction() print("keyFrameDuration=",keyFrameDuration) #for i in range (50): @@ -40,37 +42,41 @@ stage = 0 def Reset(humanoid): - global frameTime + global simTime humanoid.Reset() - frameTime = 0#random.randint(0,motion.NumFrames()-2) - print("RESET frametime=",frameTime) - humanoid.SetFrameTime(frameTime) + simTime = random.randint(0,motion.NumFrames()-2) + humanoid.SetSimTime(simTime) pose = humanoid.InitializePoseFromMotionData() - humanoid.ApplyPose(pose, True) + humanoid.ApplyPose(pose, True, True) Reset(humanoid) bc.stepSimulation() + while (1): - #frameTime = bc.readUserDebugParameter(frameTimeId) + #simTime = bc.readUserDebugParameter(frameTimeId) #print("keyFrameDuration=",keyFrameDuration) - dt = (1./240.)/keyFrameDuration - #print("dt=",dt) - frameTime += dt - if (frameTime >= (motion.NumFrames())-1.1): - frameTime = motion.NumFrames()-1.1 - #print("frameTime=", frameTime) - humanoid.SetFrameTime(frameTime) + dt = (1./240.) + print("------------------------------------------") + print("dt=",dt) + + print("simTime=",simTime) + print("humanoid.SetSimTime(simTime)") + humanoid.SetSimTime(simTime) - pose = humanoid.InitializePoseFromMotionData() + #pose = humanoid.InitializePoseFromMotionData() - #humanoid.ApplyPose(pose, False)#False, False) + #humanoid.ApplyPose(pose, True)#False)#False, False) if (humanoid.Terminates()): Reset(humanoid) - bc.stepSimulation() - - - time.sleep(1./240.) + state = humanoid.GetState() + action = [0]*36 + humanoid.ApplyAction(action) + for s in range (8): + print("step:",s) + bc.stepSimulation() + simTime += dt + time.sleep(1./240.) diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h index 98e07c98d..9bd5ff7d9 100644 --- a/src/Bullet3Common/b3Quaternion.h +++ b/src/Bullet3Common/b3Quaternion.h @@ -96,9 +96,16 @@ public: { b3Scalar d = axis.length(); b3Assert(d != b3Scalar(0.0)); - b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d; - setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s, - b3Cos(_angle * b3Scalar(0.5))); + if (d < B3_EPSILON) + { + setValue(0, 0, 0, 1); + } + else + { + b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d; + setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s, + b3Cos(_angle * b3Scalar(0.5))); + } } /**@brief Set the quaternion using Euler angles * @param yaw Angle around Y From 7669fc92c57b7e64951795f03c47267c3fe494ec Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 23 Nov 2018 18:01:00 -0800 Subject: [PATCH 05/10] add shiftCenterOfMass.py example. --- .../pybullet/examples/shiftCenterOfMass.py | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 examples/pybullet/examples/shiftCenterOfMass.py diff --git a/examples/pybullet/examples/shiftCenterOfMass.py b/examples/pybullet/examples/shiftCenterOfMass.py new file mode 100644 index 000000000..c77629b32 --- /dev/null +++ b/examples/pybullet/examples/shiftCenterOfMass.py @@ -0,0 +1,22 @@ +import pybullet as p +import time +import pybullet_data + +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + +p.loadURDF("plane.urdf") +p.setGravity(0,0,-10) +visualShift = [0,0,0] +collisionShift = [0,0,0] +inertiaShift = [0,0,-0.5] + +meshScale=[1,1,1] +visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale) +collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale) + +p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False) +while (1): + p.stepSimulation() + time.sleep(1./240.) + \ No newline at end of file From 5f0dcb575f5b85bfcaeed0f1b5f3cef35a8e5f76 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 23 Nov 2018 18:01:39 -0800 Subject: [PATCH 06/10] add DeepMimic helper utils --- .../pybullet/gym/pybullet_utils/arg_parser.py | 124 +++++++++++++++++ .../gym/pybullet_utils/examples/testlog.py | 9 ++ .../pybullet/gym/pybullet_utils/logger.py | 128 ++++++++++++++++++ .../pybullet/gym/pybullet_utils/math_util.py | 18 +++ .../pybullet/gym/pybullet_utils/mpi_util.py | 52 +++++++ examples/pybullet/gym/pybullet_utils/util.py | 13 ++ 6 files changed, 344 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_utils/arg_parser.py create mode 100644 examples/pybullet/gym/pybullet_utils/examples/testlog.py create mode 100644 examples/pybullet/gym/pybullet_utils/logger.py create mode 100644 examples/pybullet/gym/pybullet_utils/math_util.py create mode 100644 examples/pybullet/gym/pybullet_utils/mpi_util.py create mode 100644 examples/pybullet/gym/pybullet_utils/util.py diff --git a/examples/pybullet/gym/pybullet_utils/arg_parser.py b/examples/pybullet/gym/pybullet_utils/arg_parser.py new file mode 100644 index 000000000..5461a5bf2 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/arg_parser.py @@ -0,0 +1,124 @@ +import re as RE + +class ArgParser(object): + global_parser = None + + def __init__(self): + self._table = dict() + return + + def clear(self): + self._table.clear() + return + + def load_args(self, arg_strs): + succ = True + vals = [] + curr_key = '' + + for str in arg_strs: + if not (self._is_comment(str)): + is_key = self._is_key(str) + if (is_key): + if (curr_key != ''): + if (curr_key not in self._table): + self._table[curr_key] = vals + + vals = [] + curr_key = str[2::] + else: + vals.append(str) + + if (curr_key != ''): + if (curr_key not in self._table): + self._table[curr_key] = vals + + vals = [] + + return succ + + def load_file(self, filename): + succ = False + with open(filename, 'r') as file: + lines = RE.split(r'[\n\r]+', file.read()) + file.close() + + arg_strs = [] + for line in lines: + if (len(line) > 0 and not self._is_comment(line)): + arg_strs += line.split() + + succ = self.load_args(arg_strs) + return succ + + def has_key(self, key): + return key in self._table + + def parse_string(self, key, default=''): + str = default + if self.has_key(key): + str = self._table[key][0] + return str + + def parse_strings(self, key, default=[]): + arr = default + if self.has_key(key): + arr = self._table[key] + return arr + + def parse_int(self, key, default=0): + val = default + if self.has_key(key): + val = int(self._table[key][0]) + return val + + def parse_ints(self, key, default=[]): + arr = default + if self.has_key(key): + arr = [int(str) for str in self._table[key]] + return arr + + def parse_float(self, key, default=0.0): + val = default + if self.has_key(key): + val = float(self._table[key][0]) + return val + + def parse_floats(self, key, default=[]): + arr = default + if self.has_key(key): + arr = [float(str) for str in self._table[key]] + return arr + + def parse_bool(self, key, default=False): + val = default + if self.has_key(key): + val = self._parse_bool(self._table[key][0]) + return val + + def parse_bools(self, key, default=[]): + arr = default + if self.has_key(key): + arr = [self._parse_bool(str) for str in self._table[key]] + return arr + + def _is_comment(self, str): + is_comment = False + if (len(str) > 0): + is_comment = str[0] == '#' + + return is_comment + + def _is_key(self, str): + is_key = False + if (len(str) >= 3): + is_key = str[0] == '-' and str[1] == '-' + + return is_key + + def _parse_bool(self, str): + val = False + if (str == 'true' or str == 'True' or str == '1' + or str == 'T' or str == 't'): + val = True + return val \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/examples/testlog.py b/examples/pybullet/gym/pybullet_utils/examples/testlog.py new file mode 100644 index 000000000..9a275160a --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/testlog.py @@ -0,0 +1,9 @@ +from pybullet_utils.logger import Logger +logger = Logger() +logger.configure_output_file("e:/mylog.txt") +for i in range (10): + logger.log_tabular("Iteration", 1) +Logger.print2("hello world") + +logger.print_tabular() +logger.dump_tabular() \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/logger.py b/examples/pybullet/gym/pybullet_utils/logger.py new file mode 100644 index 000000000..ceb2605ba --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/logger.py @@ -0,0 +1,128 @@ +import pybullet_utils.mpi_util as MPIUtil + +""" + +Some simple logging functionality, inspired by rllab's logging. +Assumes that each diagnostic gets logged each iteration + +Call logz.configure_output_file() to start logging to a +tab-separated-values file (some_file_name.txt) + +To load the learning curves, you can do, for example + +A = np.genfromtxt('/tmp/expt_1468984536/log.txt',delimiter='\t',dtype=None, names=True) +A['EpRewMean'] + +""" + +import os.path as osp, shutil, time, atexit, os, subprocess + +class Logger: + def print2(str): + if (MPIUtil.is_root_proc()): + print(str) + return + + def __init__(self): + self.output_file = None + self.first_row = True + self.log_headers = [] + self.log_current_row = {} + self._dump_str_template = "" + return + + def reset(self): + self.first_row = True + self.log_headers = [] + self.log_current_row = {} + if self.output_file is not None: + self.output_file = open(output_path, 'w') + return + + def configure_output_file(self, filename=None): + """ + Set output directory to d, or to /tmp/somerandomnumber if d is None + """ + self.first_row = True + self.log_headers = [] + self.log_current_row = {} + + output_path = filename or "output/log_%i.txt"%int(time.time()) + + out_dir = os.path.dirname(output_path) + if not os.path.exists(out_dir) and MPIUtil.is_root_proc(): + os.makedirs(out_dir) + + if (MPIUtil.is_root_proc()): + self.output_file = open(output_path, 'w') + assert osp.exists(output_path) + atexit.register(self.output_file.close) + + Logger.print2("Logging data to " + self.output_file.name) + return + + def log_tabular(self, key, val): + """ + Log a value of some diagnostic + Call this once for each diagnostic quantity, each iteration + """ + if self.first_row and key not in self.log_headers: + self.log_headers.append(key) + else: + assert key in self.log_headers, "Trying to introduce a new key %s that you didn't include in the first iteration"%key + self.log_current_row[key] = val + return + + def get_num_keys(self): + return len(self.log_headers) + + def print_tabular(self): + """ + Print all of the diagnostics from the current iteration + """ + if (MPIUtil.is_root_proc()): + vals = [] + Logger.print2("-"*37) + for key in self.log_headers: + val = self.log_current_row.get(key, "") + if isinstance(val, float): + valstr = "%8.3g"%val + elif isinstance(val, int): + valstr = str(val) + else: + valstr = val + Logger.print2("| %15s | %15s |"%(key, valstr)) + vals.append(val) + Logger.print2("-" * 37) + return + + def dump_tabular(self): + """ + Write all of the diagnostics from the current iteration + """ + if (MPIUtil.is_root_proc()): + if (self.first_row): + self._dump_str_template = self._build_str_template() + + vals = [] + for key in self.log_headers: + val = self.log_current_row.get(key, "") + vals.append(val) + + if self.output_file is not None: + if self.first_row: + header_str = self._dump_str_template.format(*self.log_headers) + self.output_file.write(header_str + "\n") + + val_str = self._dump_str_template.format(*map(str,vals)) + self.output_file.write(val_str + "\n") + self.output_file.flush() + + self.log_current_row.clear() + self.first_row=False + return + + def _build_str_template(self): + num_keys = self.get_num_keys() + template = "{:<25}" * num_keys + return template diff --git a/examples/pybullet/gym/pybullet_utils/math_util.py b/examples/pybullet/gym/pybullet_utils/math_util.py new file mode 100644 index 000000000..d0f333b3f --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/math_util.py @@ -0,0 +1,18 @@ +import numpy as np + +RAD_TO_DEG = 57.2957795 +DEG_TO_RAD = 1.0 / RAD_TO_DEG +INVALID_IDX = -1 + +def lerp(x, y, t): + return (1 - t) * x + t * y + +def log_lerp(x, y, t): + return np.exp(lerp(np.log(x), np.log(y), t)) + +def flatten(arr_list): + return np.concatenate([np.reshape(a, [-1]) for a in arr_list], axis=0) + +def flip_coin(p): + rand_num = np.random.binomial(1, p, 1) + return rand_num[0] == 1 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/mpi_util.py b/examples/pybullet/gym/pybullet_utils/mpi_util.py new file mode 100644 index 000000000..30ca6aed0 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/mpi_util.py @@ -0,0 +1,52 @@ +import numpy as np +from mpi4py import MPI + +ROOT_PROC_RANK = 0 + +def get_num_procs(): + return MPI.COMM_WORLD.Get_size() + +def get_proc_rank(): + return MPI.COMM_WORLD.Get_rank() + +def is_root_proc(): + rank = get_proc_rank() + return rank == ROOT_PROC_RANK + +def bcast(x): + MPI.COMM_WORLD.Bcast(x, root=ROOT_PROC_RANK) + return + +def reduce_sum(x): + return reduce_all(x, MPI.SUM) + +def reduce_prod(x): + return reduce_all(x, MPI.PROD) + +def reduce_avg(x): + buffer = reduce_sum(x) + buffer /= get_num_procs() + return buffer + +def reduce_min(x): + return reduce_all(x, MPI.MIN) + +def reduce_max(x): + return reduce_all(x, MPI.MAX) + +def reduce_all(x, op): + is_array = isinstance(x, np.ndarray) + x_buf = x if is_array else np.array([x]) + buffer = np.zeros_like(x_buf) + MPI.COMM_WORLD.Allreduce(x_buf, buffer, op=op) + buffer = buffer if is_array else buffer[0] + return buffer + +def gather_all(x): + is_array = isinstance(x, np.ndarray) + x_buf = np.array([x]) + buffer = np.zeros_like(x_buf) + buffer = np.repeat(buffer, get_num_procs(), axis=0) + MPI.COMM_WORLD.Allgather(x_buf, buffer) + buffer = list(buffer) + return buffer \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/util.py b/examples/pybullet/gym/pybullet_utils/util.py new file mode 100644 index 000000000..c11ccb811 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/util.py @@ -0,0 +1,13 @@ +import random +import numpy as np + +def set_global_seeds(seed): + try: + import tensorflow as tf + except ImportError: + pass + else: + tf.set_random_seed(seed) + np.random.seed(seed) + random.seed(seed) + return \ No newline at end of file From ca36a82c62f3e69412afe88854abcbfd43f1d30f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 23 Nov 2018 18:32:23 -0800 Subject: [PATCH 07/10] add deepmimic args file (for testing) implement deepmimic reward function (only joint angles/velocities for now) fix an out-of-date comment, related to contactPoint.m_lateralFrictionInitialized in btSequentialImpulseConstraintSolver.cpp --- .../gym/pybullet_data/args/kin_char_args.txt | 4 + .../args/run_humanoid3d_backflip_args.txt | 25 ++ .../args/run_humanoid3d_cartwheel_args.txt | 25 ++ .../args/run_humanoid3d_crawl_args.txt | 25 ++ .../args/run_humanoid3d_dance_a_args.txt | 25 ++ .../args/run_humanoid3d_dance_b_args.txt | 25 ++ .../run_humanoid3d_getup_facedown_args.txt | 25 ++ .../args/run_humanoid3d_getup_faceup_args.txt | 25 ++ .../args/run_humanoid3d_jump_args.txt | 25 ++ .../args/run_humanoid3d_kick_args.txt | 25 ++ .../args/run_humanoid3d_punch_args.txt | 25 ++ .../args/run_humanoid3d_roll_args.txt | 25 ++ .../args/run_humanoid3d_run_args.txt | 25 ++ .../args/run_humanoid3d_spin_args.txt | 25 ++ .../args/run_humanoid3d_spinkick_args.txt | 25 ++ .../args/run_humanoid3d_walk_args.txt | 25 ++ .../args/train_humanoid3d_backflip_args.txt | 34 ++ .../args/train_humanoid3d_cartwheel_args.txt | 34 ++ .../args/train_humanoid3d_crawl_args.txt | 33 ++ .../args/train_humanoid3d_dance_a_args.txt | 33 ++ .../args/train_humanoid3d_dance_b_args.txt | 33 ++ .../train_humanoid3d_getup_facedown_args.txt | 34 ++ .../train_humanoid3d_getup_faceup_args.txt | 34 ++ .../args/train_humanoid3d_jump_args.txt | 33 ++ .../args/train_humanoid3d_kick_args.txt | 33 ++ .../args/train_humanoid3d_punch_args.txt | 33 ++ .../args/train_humanoid3d_roll_args.txt | 33 ++ .../args/train_humanoid3d_run_args.txt | 33 ++ .../args/train_humanoid3d_spin_args.txt | 33 ++ .../args/train_humanoid3d_spinkick_args.txt | 33 ++ .../args/train_humanoid3d_walk_args.txt | 33 ++ .../gym/pybullet_envs/mimic/humanoid.py | 327 ++++++++++++++---- .../gym/pybullet_envs/mimic/humanoid_test.py | 26 +- .../gym/pybullet_utils/examples/testargs.py | 26 ++ .../btSequentialImpulseConstraintSolver.cpp | 2 +- 35 files changed, 1174 insertions(+), 85 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_data/args/kin_char_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt create mode 100644 examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt create mode 100644 examples/pybullet/gym/pybullet_utils/examples/testargs.py diff --git a/examples/pybullet/gym/pybullet_data/args/kin_char_args.txt b/examples/pybullet/gym/pybullet_data/args/kin_char_args.txt new file mode 100644 index 000000000..aad477912 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/kin_char_args.txt @@ -0,0 +1,4 @@ +--scene kin_char + +--character_file data/characters/humanoid3d.txt +--motion_file data/motions/humanoid3d_spinkick.txt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt new file mode 100644 index 000000000..ebf17ae56 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_backflip.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_backflip.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt new file mode 100644 index 000000000..65a43595c --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 9 10 12 13 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_cartwheel.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_cartwheel.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt new file mode 100644 index 000000000..ac0f6e3aa --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_crawl.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_crawl.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt new file mode 100644 index 000000000..3dbce45e3 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_dance_a.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_dance_a.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt new file mode 100644 index 000000000..acbe85d7f --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_dance_b.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_dance_b.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt new file mode 100644 index 000000000..1326c2734 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--enable_char_contact_fall false + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_getup_facedown.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_getup_facedown.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt new file mode 100644 index 000000000..d36661ac3 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--enable_char_contact_fall false + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_getup_faceup.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_getup_faceup.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt new file mode 100644 index 000000000..2b1eb42c5 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_jump.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_jump.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt new file mode 100644 index 000000000..2d946ceeb --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_kick.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_kick.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt new file mode 100644 index 000000000..31c1bef66 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_punch.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_punch.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt new file mode 100644 index 000000000..12ecf0114 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--enable_char_contact_fall false + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_roll.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_roll.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt new file mode 100644 index 000000000..84019d705 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_run.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_run.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt new file mode 100644 index 000000000..ce8d569b5 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_spin.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_spin.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt new file mode 100644 index 000000000..95fcd02f7 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_spinkick.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_spinkick.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt new file mode 100644 index 000000000..dab89ba2b --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt @@ -0,0 +1,25 @@ +--scene imitate + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_walk.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +--train_agents false + +#--output_path output +#--int_output_path output/intermediate +--model_files data/policies/humanoid3d/humanoid3d_walk.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt new file mode 100644 index 000000000..3e069f29d --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt @@ -0,0 +1,34 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_backflip.txt +--sync_char_root_pos true +--sync_char_root_rot false +--enable_root_rot_fail false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt new file mode 100644 index 000000000..21850cb2a --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt @@ -0,0 +1,34 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 9 10 12 13 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_cartwheel.txt +--sync_char_root_pos true +--sync_char_root_rot false +--enable_root_rot_fail false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt new file mode 100644 index 000000000..c64f45adf --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_crawl.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt new file mode 100644 index 000000000..784ed728a --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_dance_a.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt new file mode 100644 index 000000000..afef9a240 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_dance_b.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt new file mode 100644 index 000000000..b1af6b78e --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt @@ -0,0 +1,34 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--enable_char_contact_fall false +--hold_end_frame 0.3 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_getup_facedown.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt new file mode 100644 index 000000000..1d36937ca --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt @@ -0,0 +1,34 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--enable_char_contact_fall false +--hold_end_frame 0.3 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_getup_faceup.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt new file mode 100644 index 000000000..23cc690c9 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_jump.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt new file mode 100644 index 000000000..94359b039 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 4 +--time_end_lim_max 4 +--time_end_lim_exp 10 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_kick.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt new file mode 100644 index 000000000..f6e14bf8b --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 4 +--time_end_lim_max 4 +--time_end_lim_exp 10 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_punch.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt new file mode 100644 index 000000000..1184afe11 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--enable_char_contact_fall false + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_roll.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt new file mode 100644 index 000000000..2971efa6b --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_run.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt new file mode 100644 index 000000000..7795cc7cf --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_spin.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt new file mode 100644 index 000000000..c38bb62e0 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_spinkick.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt new file mode 100644 index 000000000..54f544971 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt @@ -0,0 +1,33 @@ +--scene imitate + +--time_lim_min 0.5 +--time_lim_max 0.5 +--time_lim_exp 0.2 +--time_end_lim_min 20 +--time_end_lim_max 20 +--time_end_lim_exp 50 +--anneal_samples 32000000 + +--num_update_substeps 10 +--num_sim_substeps 2 +--world_scale 4 + +--terrain_file data/terrain/plane.txt + +--char_types general +--character_files data/characters/humanoid3d.txt +--enable_char_soft_contact false +--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14 + +--char_ctrls ct_pd +--char_ctrl_files data/controllers/humanoid3d_ctrl.txt +--motion_file data/motions/humanoid3d_walk.txt +--sync_char_root_pos true +--sync_char_root_rot false + +--agent_files data/agents/ct_agent_humanoid_ppo.txt +#--train_agents false + +--output_path output +#--int_output_path output/intermediate +#--model_files data/policies/humanoid3d/agent0_model.ckpt \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py index c3b0d0970..2703aa8ea 100644 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py +++ b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py @@ -4,6 +4,9 @@ currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfram parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) +from pybullet_utils.bullet_client import BulletClient +import pybullet_data + jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] @@ -166,14 +169,24 @@ class Humanoid(object): """ self._baseShift = baseShift self._pybullet_client = pybullet_client + + self.kin_client = BulletClient(pybullet_client.DIRECT)# use SHARED_MEMORY for visual debugging, start a GUI physics server first + self.kin_client.resetSimulation() + self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath()) + self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP,1) + self.kin_client.setGravity(0,-9.8,0) + self._motion_data = motion_data self._humanoid = self._pybullet_client.loadURDF( "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False) + self._kinematicHumanoid = self.kin_client.loadURDF( + "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False) + #self._humanoidDebug = self._pybullet_client.loadURDF( # "humanoid/humanoid.urdf", [0,0.9,3],globalScaling=0.25, useFixedBase=True) - print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid)) + #print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid)) pose = HumanoidPose() for i in range (self._motion_data.NumFrames()-1): @@ -186,8 +199,8 @@ class Humanoid(object): ji = self._pybullet_client.getJointInfo(self._humanoid,j) self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0) self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,1]) - print("joint[",j,"].type=",jointTypes[ji[2]]) - print("joint[",j,"].name=",ji[1]) + #print("joint[",j,"].type=",jointTypes[ji[2]]) + #print("joint[",j,"].name=",ji[1]) self._initial_state = self._pybullet_client.saveState() self._allowed_body_parts=[11,14] @@ -195,9 +208,9 @@ class Humanoid(object): def Reset(self): self._pybullet_client.restoreState(self._initial_state) - self.SetSimTime(100) + self.SetSimTime(0) pose = self.InitializePoseFromMotionData() - self.ApplyPose(pose, True, True) + self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client) def CalcCycleCount(self, simTime, cycleTime): phases = simTime / cycleTime; @@ -208,7 +221,7 @@ class Humanoid(object): def SetSimTime(self, t): self._simTime = t - print("SetTimeTime time =",t) + #print("SetTimeTime time =",t) keyFrameDuration = self._motion_data.KeyFrameDuraction() cycleTime = keyFrameDuration*(self._motion_data.NumFrames()-1) #print("self._motion_data.NumFrames()=",self._motion_data.NumFrames()) @@ -253,7 +266,7 @@ class Humanoid(object): rotVec = self._pybullet_client.rotateVector(rootOrn, refDir) heading = math.atan2(-rotVec[2], rotVec[0]) heading2=eul[1] - print("heading=",heading) + #print("heading=",heading) headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading) return headingOrn @@ -269,21 +282,21 @@ class Humanoid(object): def BuildOriginTrans(self): rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) - print("rootPos=",rootPos, " rootOrn=",rootOrn) + #print("rootPos=",rootPos, " rootOrn=",rootOrn) invRootPos=[-rootPos[0], 0, -rootPos[2]] #invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn) headingOrn = self.BuildHeadingTrans(rootOrn) - print("headingOrn=",headingOrn) + #print("headingOrn=",headingOrn) headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn) - print("headingMat=",headingMat) + #print("headingMat=",headingMat) #dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn) #dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn) invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms( [0,0,0],headingOrn, invRootPos,[0,0,0,1]) - print("invOrigTransPos=",invOrigTransPos) - print("invOrigTransOrn=",invOrigTransOrn) + #print("invOrigTransPos=",invOrigTransPos) + #print("invOrigTransOrn=",invOrigTransOrn) invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn) - print("invOrigTransMat =",invOrigTransMat ) + #print("invOrigTransMat =",invOrigTransMat ) return invOrigTransPos, invOrigTransOrn def InitializePoseFromMotionData(self): @@ -360,28 +373,30 @@ class Humanoid(object): pose._leftElbowRot = [angle] - print("index=",index) + #print("index=",index) initializeBase = False initializeVelocities = False - self.ApplyPose(pose, initializeBase, initializeVelocities) + self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid, self._pybullet_client) - def ApplyPose(self, pose, initializeBase, initializeVelocities): + def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid,bc): #todo: get tunable parametes from a json file or from URDF (kd, maxForce) if (initializeBase): - self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,0,0,1]) + bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,0,0,1]) basePos=[pose._basePos[0]+self._baseShift[0],pose._basePos[1]+self._baseShift[1],pose._basePos[2]+self._baseShift[2]] - self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, + bc.resetBasePositionAndOrientation(humanoid, basePos, pose._baseOrn) if initializeVelocities: - self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel) + bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel) #print("resetBaseVelocity=",pose._baseLinVel) else: - self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1]) + bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,1,1,1]) - kp=0.01 + + + kp=0.03 chest=1 neck=2 rightShoulder=3 @@ -394,63 +409,63 @@ class Humanoid(object): leftHip = 12 leftKnee=13 leftAnkle=14 - controlMode = self._pybullet_client.POSITION_CONTROL + controlMode = bc.POSITION_CONTROL if (initializeBase): if initializeVelocities: - self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot, pose._chestVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot, pose._neckVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot, pose._rightHipVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot, pose._leftHipVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel) + bc.resetJointStateMultiDof(humanoid,chest,pose._chestRot, pose._chestVel) + bc.resetJointStateMultiDof(humanoid,neck,pose._neckRot, pose._neckVel) + bc.resetJointStateMultiDof(humanoid,rightHip,pose._rightHipRot, pose._rightHipVel) + bc.resetJointStateMultiDof(humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel) + bc.resetJointStateMultiDof(humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel) + bc.resetJointStateMultiDof(humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel) + bc.resetJointStateMultiDof(humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel) + bc.resetJointStateMultiDof(humanoid,leftHip, pose._leftHipRot, pose._leftHipVel) + bc.resetJointStateMultiDof(humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel) + bc.resetJointStateMultiDof(humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel) + bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel) + bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel) else: - self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot) - self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot) + bc.resetJointStateMultiDof(humanoid,chest,pose._chestRot) + bc.resetJointStateMultiDof(humanoid,neck,pose._neckRot) + bc.resetJointStateMultiDof(humanoid,rightHip,pose._rightHipRot) + bc.resetJointStateMultiDof(humanoid,rightKnee,pose._rightKneeRot) + bc.resetJointStateMultiDof(humanoid,rightAnkle,pose._rightAnkleRot) + bc.resetJointStateMultiDof(humanoid,rightShoulder,pose._rightShoulderRot) + bc.resetJointStateMultiDof(humanoid,rightElbow, pose._rightElbowRot) + bc.resetJointStateMultiDof(humanoid,leftHip, pose._leftHipRot) + bc.resetJointStateMultiDof(humanoid,leftKnee, pose._leftKneeRot) + bc.resetJointStateMultiDof(humanoid,leftAnkle, pose._leftAnkleRot) + bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot) + bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100) - self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60) + bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200) + bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50) + bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200) + bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150) + bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90) + bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100) + bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60) + bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200) + bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150) + bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90) + bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100) + bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60) #debug space #if (False): - # for j in range (self._pybullet_client.getNumJoints(self._humanoid)): - # js = self._pybullet_client.getJointState(self._humanoid, j) - # self._pybullet_client.resetJointState(self._humanoidDebug, j,js[0]) - # jsm = self._pybullet_client.getJointStateMultiDof(self._humanoid, j) + # for j in range (bc.getNumJoints(self._humanoid)): + # js = bc.getJointState(self._humanoid, j) + # bc.resetJointState(self._humanoidDebug, j,js[0]) + # jsm = bc.getJointStateMultiDof(self._humanoid, j) # if (len(jsm[0])>0): - # self._pybullet_client.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0]) + # bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0]) def GetState(self): stateVector = [] phase = self.GetPhase() - print("phase=",phase) + #print("phase=",phase) stateVector.append(phase) rootTransPos, rootTransOrn=self.BuildOriginTrans() @@ -459,16 +474,19 @@ class Humanoid(object): rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1]) #print("!!!rootPosRel =",rootPosRel ) #print("rootTransPos=",rootTransPos) - print("basePos=",basePos) + #print("basePos=",basePos) localPos,localOrn = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn , basePos,baseOrn ) localPos=[localPos[0]-rootPosRel[0],localPos[1]-rootPosRel[1],localPos[2]-rootPosRel[2]] - print("localPos=",localPos) + #print("localPos=",localPos) stateVector.append(rootPosRel[1]) + self.pb2dmJoints=[0,1,2,9,10,11,3,4,5,12,13,14,6,7,8] - for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + for pbJoint in range (self._pybullet_client.getNumJoints(self._humanoid)): + j = self.pb2dmJoints[pbJoint] + #print("joint order:",j) ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True) linkPos = ls[0] linkOrn = ls[1] @@ -478,11 +496,16 @@ class Humanoid(object): linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]] for l in linkPosLocal: stateVector.append(l) - for l in linkOrnLocal: - stateVector.append(l) + + #re-order the quaternion, DeepMimic uses w,x,y,z + stateVector.append(linkOrnLocal[3]) + stateVector.append(linkOrnLocal[0]) + stateVector.append(linkOrnLocal[1]) + stateVector.append(linkOrnLocal[2]) - for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + for pbJoint in range (self._pybullet_client.getNumJoints(self._humanoid)): + j = self.pb2dmJoints[pbJoint] ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True) linkLinVel = ls[6] linkAngVel = ls[7] @@ -491,7 +514,167 @@ class Humanoid(object): for l in linkAngVel: stateVector.append(l) - print("stateVector len=",len(stateVector)) + #print("stateVector len=",len(stateVector)) + #for st in range (len(stateVector)): + # print("state[",st,"]=",stateVector[st]) return stateVector - \ No newline at end of file + + def GetReward(self): + #from DeepMimic double cSceneImitate::CalcRewardImitate + pose_w = 0.5 + vel_w = 0.05 + end_eff_w = 0 #0.15 + root_w = 0 #0.2 + com_w = 0.1 + + total_w = pose_w + vel_w + end_eff_w + root_w + com_w + pose_w /= total_w + vel_w /= total_w + end_eff_w /= total_w + root_w /= total_w + com_w /= total_w + + pose_scale = 2 + vel_scale = 0.1 + end_eff_scale = 40 + root_scale = 5 + com_scale = 10 + err_scale = 1 + + reward = 0 + + pose_err = 0 + vel_err = 0 + end_eff_err = 0 + root_err = 0 + com_err = 0 + heading_err = 0 + + #create a mimic reward, comparing the dynamics humanoid with a kinematic one + + pose = self.InitializePoseFromMotionData() + #print("self._kinematicHumanoid=",self._kinematicHumanoid) + #print("kinematicHumanoid #joints=",self.kin_client.getNumJoints(self._kinematicHumanoid)) + self.ApplyPose(pose, True, True, self._kinematicHumanoid, self.kin_client) + + #const Eigen::VectorXd& pose0 = sim_char.GetPose(); + #const Eigen::VectorXd& vel0 = sim_char.GetVel(); + #const Eigen::VectorXd& pose1 = kin_char.GetPose(); + #const Eigen::VectorXd& vel1 = kin_char.GetVel(); + #tMatrix origin_trans = sim_char.BuildOriginTrans(); + #tMatrix kin_origin_trans = kin_char.BuildOriginTrans(); + # + #tVector com0_world = sim_char.CalcCOM(); + #tVector com_vel0_world = sim_char.CalcCOMVel(); + #tVector com1_world; + #tVector com_vel1_world; + #cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world); + # + root_id = 0 + #tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0); + #tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1); + #tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0); + #tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1); + #tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0); + #tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1); + #tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0); + #tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1); + + mJointWeights = [0.20833,0.10416, 0.0625, 0.10416, + 0.0625, 0.041666666666666671, 0.0625, 0.0416, + 0.00, 0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000] + + num_end_effs = 0 + num_joints = 15 + + root_rot_w = mJointWeights[root_id] + #pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1) + #vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1) + + for j in range (num_joints): + curr_pose_err = 0 + curr_vel_err = 0 + w = mJointWeights[j]; + + simJointInfo = self._pybullet_client.getJointStateMultiDof(self._humanoid, j) + + #print("simJointInfo.pos=",simJointInfo[0]) + #print("simJointInfo.vel=",simJointInfo[1]) + kinJointInfo = self.kin_client.getJointStateMultiDof(self._kinematicHumanoid,j) + #print("kinJointInfo.pos=",kinJointInfo[0]) + #print("kinJointInfo.vel=",kinJointInfo[1]) + if (len(simJointInfo[0])==1): + angle = simJointInfo[0][0]-kinJointInfo[0][0] + curr_pose_err = angle*angle + velDiff = simJointInfo[1][0]-kinJointInfo[1][0] + curr_vel_err = velDiff*velDiff + if (len(simJointInfo[0])==4): + #print("quaternion diff") + diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0],kinJointInfo[0]) + axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat) + curr_pose_err = angle*angle + diffVel = [simJointInfo[1][0]-kinJointInfo[1][0],simJointInfo[1][1]-kinJointInfo[1][1],simJointInfo[1][2]-kinJointInfo[1][2]] + curr_vel_err = diffVel[0]*diffVel[0]+diffVel[1]*diffVel[1]+diffVel[2]*diffVel[2] + + + pose_err += w * curr_pose_err + vel_err += w * curr_vel_err + + # bool is_end_eff = sim_char.IsEndEffector(j) + # if (is_end_eff) + # { + # tVector pos0 = sim_char.CalcJointPos(j) + # tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j) + # double ground_h0 = mGround->SampleHeight(pos0) + # double ground_h1 = kin_char.GetOriginPos()[1] + # + # tVector pos_rel0 = pos0 - root_pos0 + # tVector pos_rel1 = pos1 - root_pos1 + # pos_rel0[1] = pos0[1] - ground_h0 + # pos_rel1[1] = pos1[1] - ground_h1 + # + # pos_rel0 = origin_trans * pos_rel0 + # pos_rel1 = kin_origin_trans * pos_rel1 + # + # curr_end_err = (pos_rel1 - pos_rel0).squaredNorm() + # end_eff_err += curr_end_err; + # ++num_end_effs; + # } + #} + #if (num_end_effs > 0): + # end_eff_err /= num_end_effs + # + #double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos()) + #double root_ground_h1 = kin_char.GetOriginPos()[1] + #root_pos0[1] -= root_ground_h0 + #root_pos1[1] -= root_ground_h1 + #root_pos_err = (root_pos0 - root_pos1).squaredNorm() + # + #root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1) + #root_rot_err *= root_rot_err + + #root_vel_err = (root_vel1 - root_vel0).squaredNorm() + #root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm() + + #root_err = root_pos_err + # + 0.1 * root_rot_err + # + 0.01 * root_vel_err + # + 0.001 * root_ang_vel_err + #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm() + + #print("pose_err=",pose_err) + #print("vel_err=",vel_err) + pose_reward = math.exp(-err_scale * pose_scale * pose_err) + vel_reward = math.exp(-err_scale * vel_scale * vel_err) + end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err) + root_reward = math.exp(-err_scale * root_scale * root_err) + com_reward = math.exp(-err_scale * com_scale * com_err) + + reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward + + #print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward, + # pose_reward,vel_reward,end_eff_reward, root_reward, com_reward); + + + return reward \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py index b64ec71ea..2c634e018 100644 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py +++ b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py @@ -19,19 +19,19 @@ motion=MotionCaptureData() motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt" motion.Load(motionPath) -print("numFrames = ", motion.NumFrames()) +#print("numFrames = ", motion.NumFrames()) simTimeId= bc.addUserDebugParameter("simTime",0,motion.NumFrames()-1.1,0) y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0]) bc.loadURDF("plane.urdf",[0,-0.04,0], y2zOrn) -humanoid = Humanoid(bc, motion,[0,0,0]) +humanoid = Humanoid(bc, motion,[0,0,0])#4000,0,5000]) simTime = 0 keyFrameDuration = motion.KeyFrameDuraction() -print("keyFrameDuration=",keyFrameDuration) +#print("keyFrameDuration=",keyFrameDuration) #for i in range (50): # bc.stepSimulation() @@ -44,25 +44,25 @@ stage = 0 def Reset(humanoid): global simTime humanoid.Reset() - simTime = random.randint(0,motion.NumFrames()-2) + simTime = 0 #random.randint(0,motion.NumFrames()-2) humanoid.SetSimTime(simTime) pose = humanoid.InitializePoseFromMotionData() - humanoid.ApplyPose(pose, True, True) + humanoid.ApplyPose(pose, True, True, humanoid._humanoid,bc) Reset(humanoid) -bc.stepSimulation() +#bc.stepSimulation() while (1): #simTime = bc.readUserDebugParameter(frameTimeId) #print("keyFrameDuration=",keyFrameDuration) dt = (1./240.) - print("------------------------------------------") - print("dt=",dt) + #print("------------------------------------------") + #print("dt=",dt) - print("simTime=",simTime) - print("humanoid.SetSimTime(simTime)") + #print("simTime=",simTime) + #print("humanoid.SetSimTime(simTime)") humanoid.SetSimTime(simTime) #pose = humanoid.InitializePoseFromMotionData() @@ -75,8 +75,10 @@ while (1): action = [0]*36 humanoid.ApplyAction(action) for s in range (8): - print("step:",s) + #print("step:",s) bc.stepSimulation() simTime += dt time.sleep(1./240.) - + reward = humanoid.GetReward() + print("reward=",reward) + diff --git a/examples/pybullet/gym/pybullet_utils/examples/testargs.py b/examples/pybullet/gym/pybullet_utils/examples/testargs.py new file mode 100644 index 000000000..d03a82041 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/testargs.py @@ -0,0 +1,26 @@ +import pybullet_data +from pybullet_utils.arg_parser import ArgParser +from pybullet_utils.logger import Logger +import sys + +def build_arg_parser(args): + arg_parser = ArgParser() + arg_parser.load_args(args) + + arg_file = arg_parser.parse_string('arg_file', '') + if (arg_file != ''): + path = pybullet_data.getDataPath()+"/args/"+arg_file + succ = arg_parser.load_file(path) + Logger.print2(arg_file) + assert succ, Logger.print2('Failed to load args from: ' + arg_file) + + return arg_parser + +args = sys.argv[1:] +arg_parser = build_arg_parser(args) +motion_file = arg_parser.parse_string("motion_file") +print("motion_file=",motion_file) +bodies = arg_parser.parse_ints("fall_contact_bodies") +print("bodies=",bodies) +int_output_path = arg_parser.parse_string("int_output_path") +print("int_output_path=",int_output_path) \ No newline at end of file diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e8a766ecc..89c163e45 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -1110,7 +1110,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// ///The user can manually override the friction directions for certain contacts using a contact callback, - ///and set the cp.m_lateralFrictionInitialized to true + ///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// From bb6f4fb17ced07e49181daad54b4a525c41fcfa1 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 24 Nov 2018 11:54:36 -0800 Subject: [PATCH 08/10] improve the ARS implementation: add multiprocessing Gym environment stepping, add command-line parameters to resume a policy, --render, --movie, --steps, --env --- .../pybullet/gym/pybullet_envs/ARS/ars.py | 191 +++++++++++++++--- 1 file changed, 160 insertions(+), 31 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py index b4c0c3127..92189d5d5 100644 --- a/examples/pybullet/gym/pybullet_envs/ARS/ars.py +++ b/examples/pybullet/gym/pybullet_envs/ARS/ars.py @@ -6,13 +6,17 @@ import numpy as np import gym from gym import wrappers import pybullet_envs +import time +import multiprocessing as mp +from multiprocessing import Process, Pipe +import argparse + # Setting the Hyper Parameters - class Hp(): def __init__(self): - self.nb_steps = 1000 + self.nb_steps = 10000 self.episode_length = 1000 self.learning_rate = 0.02 self.nb_directions = 16 @@ -22,6 +26,58 @@ class Hp(): self.seed = 1 self.env_name = 'HalfCheetahBulletEnv-v0' + +# Multiprocess Exploring the policy on one specific direction and over one episode + +_RESET = 1 +_CLOSE = 2 +_EXPLORE = 3 + +def ExploreWorker(rank,childPipe, envname, args): + env = gym.make(envname) + nb_inputs = env.observation_space.shape[0] + normalizer = Normalizer(nb_inputs) + observation_n = env.reset() + n=0 + while True: + n+=1 + try: + # Only block for short times to have keyboard exceptions be raised. + if not childPipe.poll(0.001): + continue + message, payload = childPipe.recv() + except (EOFError, KeyboardInterrupt): + break + if message == _RESET: + observation_n = env.reset() + childPipe.send(["reset ok"]) + continue + if message == _EXPLORE: + #normalizer = payload[0] #use our local normalizer + policy = payload[1] + hp = payload[2] + direction = payload[3] + delta = payload[4] + state = env.reset() + done = False + num_plays = 0. + sum_rewards = 0 + while not done and num_plays < hp.episode_length: + normalizer.observe(state) + state = normalizer.normalize(state) + action = policy.evaluate(state, delta, direction,hp) + state, reward, done, _ = env.step(action) + reward = max(min(reward, 1), -1) + sum_rewards += reward + num_plays += 1 + childPipe.send([sum_rewards]) + continue + if message == _CLOSE: + childPipe.send(["close ok"]) + break + childPipe.close() + + # Normalizing the states class Normalizer(): @@ -47,11 +103,14 @@ class Normalizer(): # Building the AI class Policy(): - - def __init__(self, input_size, output_size): - self.theta = np.zeros((output_size, input_size)) - print("self.theta=",self.theta) - def evaluate(self, input, delta = None, direction = None): + def __init__(self, input_size, output_size, env_name, args): + try: + self.theta = np.load(args.policy) + except: + self.theta = np.zeros((output_size, input_size)) + self.env_name = env_name + print("Starting policy theta=",self.theta) + def evaluate(self, input, delta, direction, hp): if direction is None: return np.clip(self.theta.dot(input), -1.0, 1.0) elif direction == "positive": @@ -62,15 +121,18 @@ class Policy(): def sample_deltas(self): return [np.random.randn(*self.theta.shape) for _ in range(hp.nb_directions)] - def update(self, rollouts, sigma_r): + def update(self, rollouts, sigma_r, args): step = np.zeros(self.theta.shape) for r_pos, r_neg, d in rollouts: step += (r_pos - r_neg) * d self.theta += hp.learning_rate / (hp.nb_best_directions * sigma_r) * step + timestr = time.strftime("%Y%m%d-%H%M%S") + np.save(args.logdir+"/policy_"+self.env_name+"_"+timestr+".npy", self.theta) + # Exploring the policy on one specific direction and over one episode -def explore(env, normalizer, policy, direction = None, delta = None): +def explore(env, normalizer, policy, direction, delta, hp): state = env.reset() done = False num_plays = 0. @@ -78,7 +140,7 @@ def explore(env, normalizer, policy, direction = None, delta = None): while not done and num_plays < hp.episode_length: normalizer.observe(state) state = normalizer.normalize(state) - action = policy.evaluate(state, delta, direction) + action = policy.evaluate(state, delta, direction, hp) state, reward, done, _ = env.step(action) reward = max(min(reward, 1), -1) sum_rewards += reward @@ -87,7 +149,7 @@ def explore(env, normalizer, policy, direction = None, delta = None): # Training the AI -def train(env, policy, normalizer, hp): +def train(env, policy, normalizer, hp, parentPipes, args): for step in range(hp.nb_steps): @@ -96,13 +158,29 @@ def train(env, policy, normalizer, hp): positive_rewards = [0] * hp.nb_directions negative_rewards = [0] * hp.nb_directions - # Getting the positive rewards in the positive directions - for k in range(hp.nb_directions): - positive_rewards[k] = explore(env, normalizer, policy, direction = "positive", delta = deltas[k]) + if parentPipes: + for k in range(hp.nb_directions): + parentPipe = parentPipes[k] + parentPipe.send([_EXPLORE,[normalizer, policy, hp, "positive", deltas[k]]]) + for k in range(hp.nb_directions): + positive_rewards[k] = parentPipes[k].recv()[0] + + for k in range(hp.nb_directions): + parentPipe = parentPipes[k] + parentPipe.send([_EXPLORE,[normalizer, policy, hp, "negative", deltas[k]]]) + for k in range(hp.nb_directions): + negative_rewards[k] = parentPipes[k].recv()[0] + + else: + # Getting the positive rewards in the positive directions + for k in range(hp.nb_directions): + positive_rewards[k] = explore(env, normalizer, policy, "positive", deltas[k], hp) - # Getting the negative rewards in the negative/opposite directions - for k in range(hp.nb_directions): - negative_rewards[k] = explore(env, normalizer, policy, direction = "negative", delta = deltas[k]) + + # Getting the negative rewards in the negative/opposite directions + for k in range(hp.nb_directions): + negative_rewards[k] = explore(env, normalizer, policy, "negative", deltas[k], hp) + # Gathering all the positive/negative rewards to compute the standard deviation of these rewards all_rewards = np.array(positive_rewards + negative_rewards) @@ -114,10 +192,10 @@ def train(env, policy, normalizer, hp): rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k]) for k in order] # Updating our policy - policy.update(rollouts, sigma_r) + policy.update(rollouts, sigma_r, args) # Printing the final reward of the policy after the update - reward_evaluation = explore(env, normalizer, policy) + reward_evaluation = explore(env, normalizer, policy, None, None, hp) print('Step:', step, 'Reward:', reward_evaluation) # Running the main code @@ -127,16 +205,67 @@ def mkdir(base, name): if not os.path.exists(path): os.makedirs(path) return path -work_dir = mkdir('exp', 'brs') -monitor_dir = mkdir(work_dir, 'monitor') -hp = Hp() -np.random.seed(hp.seed) -env = gym.make(hp.env_name) -# env.render(mode = "human") -#env = wrappers.Monitor(env, monitor_dir, force = True) -nb_inputs = env.observation_space.shape[0] -nb_outputs = env.action_space.shape[0] -policy = Policy(nb_inputs, nb_outputs) -normalizer = Normalizer(nb_inputs) -train(env, policy, normalizer, hp) + + + +if __name__ == "__main__": + mp.freeze_support() + + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--env', help='Gym environment name', type=str, default='HalfCheetahBulletEnv-v0') + parser.add_argument('--seed', help='RNG seed', type=int, default=1) + parser.add_argument('--render', help='OpenGL Visualizer', type=int, default=0) + parser.add_argument('--movie',help='rgb_array gym movie',type=int, default=0) + parser.add_argument('--steps', help='Number of steps', type=int, default=10000) + parser.add_argument('--policy', help='Starting policy file (npy)', type=str, default='') + parser.add_argument('--logdir', help='Directory root to log policy files (npy)', type=str, default='.') + parser.add_argument('--mp', help='Enable multiprocessing', type=int, default=1) + + args = parser.parse_args() + + hp = Hp() + hp.env_name = args.env + hp.seed = args.seed + hp.nb_steps = args.steps + print("seed = ", hp.seed) + np.random.seed(hp.seed) + + parentPipes = None + if args.mp: + num_processes = hp.nb_directions + processes = [] + childPipes = [] + parentPipes = [] + + for pr in range (num_processes): + parentPipe, childPipe = Pipe() + parentPipes.append(parentPipe) + childPipes.append(childPipe) + + for rank in range(num_processes): + p = mp.Process(target=ExploreWorker, args=(rank,childPipes[rank], hp.env_name, args)) + p.start() + processes.append(p) + + work_dir = mkdir('exp', 'brs') + monitor_dir = mkdir(work_dir, 'monitor') + env = gym.make(hp.env_name) + if args.render: + env.render(mode = "human") + if args.movie: + env = wrappers.Monitor(env, monitor_dir, force = True) + nb_inputs = env.observation_space.shape[0] + nb_outputs = env.action_space.shape[0] + policy = Policy(nb_inputs, nb_outputs,hp.env_name, args) + normalizer = Normalizer(nb_inputs) + + print("start training") + train(env, policy, normalizer, hp, parentPipes, args) + + if args.mp: + for parentPipe in parentPipes: + parentPipe.send([_CLOSE,"pay2"]) + + for p in processes: + p.join() From cf48697fcfd4eba26c634755afd2538f940a88ee Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 24 Nov 2018 11:57:01 -0800 Subject: [PATCH 09/10] PyBullet on Windows: disable harmless warning of not being able to connect to shared memory server. --- examples/SharedMemory/Win32SharedMemory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/Win32SharedMemory.cpp b/examples/SharedMemory/Win32SharedMemory.cpp index 5e973ae5e..53fdad102 100644 --- a/examples/SharedMemory/Win32SharedMemory.cpp +++ b/examples/SharedMemory/Win32SharedMemory.cpp @@ -91,7 +91,7 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreat } else { - b3Warning("Could not create file mapping object (%d).\n", GetLastError()); + //b3Warning("Could not create file mapping object (%d).\n", GetLastError()); return 0; } } From 2e30a9565b4cde912f5c632aff30b54200168cbc Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 25 Nov 2018 15:33:28 -0800 Subject: [PATCH 10/10] add humanoid_deepmimic_gym_env.py for HumanoidDeepMimicBulletEnv-v1 (still untested) --- .../pybullet/gym/pybullet_envs/ARS/ars.py | 6 + .../pybullet/gym/pybullet_envs/__init__.py | 7 + .../gym/pybullet_envs/deep_mimic/__init__.py | 1 + .../{mimic => deep_mimic}/humanoid.py | 11 +- .../deep_mimic/humanoid_deepmimic_gym_env.py | 277 ++++++++++++++++++ .../{mimic => deep_mimic}/humanoid_test.py | 7 +- .../motion_capture_data.py | 0 7 files changed, 303 insertions(+), 6 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py rename examples/pybullet/gym/pybullet_envs/{mimic => deep_mimic}/humanoid.py (99%) create mode 100644 examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py rename examples/pybullet/gym/pybullet_envs/{mimic => deep_mimic}/humanoid_test.py (91%) rename examples/pybullet/gym/pybullet_envs/{mimic => deep_mimic}/motion_capture_data.py (100%) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py index 92189d5d5..760ebd732 100644 --- a/examples/pybullet/gym/pybullet_envs/ARS/ars.py +++ b/examples/pybullet/gym/pybullet_envs/ARS/ars.py @@ -1,5 +1,11 @@ # AI 2018 +import os +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + # Importing the libraries import os import numpy as np diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index d2a5a95b4..974001b79 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -8,6 +8,13 @@ def register(id,*args,**kvargs): # ------------bullet------------- +register( + id='HumanoidDeepMimicBulletEnv-v1', + entry_point='pybullet_envs.deep_mimic:HumanoidDeepMimicGymEnv', + max_episode_steps=1000, + reward_threshold=20000.0, +) + register( id='CartPoleBulletEnv-v1', entry_point='pybullet_envs.bullet:CartPoleBulletEnv', diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py new file mode 100644 index 000000000..f2b2400ca --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py @@ -0,0 +1 @@ +from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py similarity index 99% rename from examples/pybullet/gym/pybullet_envs/mimic/humanoid.py rename to examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py index 2703aa8ea..b2ebf0994 100644 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py @@ -177,14 +177,13 @@ class Humanoid(object): self.kin_client.setGravity(0,-9.8,0) self._motion_data = motion_data + print("LOADING humanoid!") self._humanoid = self._pybullet_client.loadURDF( "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False) self._kinematicHumanoid = self.kin_client.loadURDF( "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False) - #self._humanoidDebug = self._pybullet_client.loadURDF( - # "humanoid/humanoid.urdf", [0,0.9,3],globalScaling=0.25, useFixedBase=True) #print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid)) pose = HumanoidPose() @@ -676,5 +675,9 @@ class Humanoid(object): #print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward, # pose_reward,vel_reward,end_eff_reward, root_reward, com_reward); - - return reward \ No newline at end of file + return reward + + def GetBasePosition(self): + pos,orn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) + return pos + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py new file mode 100644 index 000000000..9e1a674f2 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py @@ -0,0 +1,277 @@ +"""This file implements the gym environment of humanoid deepmimic using PyBullet. + +""" +import math +import time + +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +import gym +from gym import spaces +from gym.utils import seeding +import random +import numpy as np +import pybullet +import pybullet_data +from pybullet_envs.deep_mimic.humanoid import Humanoid +from pkg_resources import parse_version +from pybullet_utils import bullet_client +from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData + +RENDER_HEIGHT = 360 +RENDER_WIDTH = 480 + + +class HumanoidDeepMimicGymEnv(gym.Env): + """The gym environment for the humanoid deep mimic. + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 100 + } + + def __init__(self, + urdf_root=pybullet_data.getDataPath(), + render=False): + """Initialize the gym environment. + + Args: + urdf_root: The path to the urdf data folder. + render: Whether to render the simulation. + Raises: + ValueError: If the urdf_version is not supported. + """ + # Set up logging. + self._urdf_root = urdf_root + self._observation = [] + self._env_step_counter = 0 + self._is_render = render + self._cam_dist = 1.0 + self._cam_yaw = 0 + self._cam_pitch = -30 + self._ground_id = None + self._pybullet_client = None + self._humanoid = None + self._control_time_step = 8.*(1./240.)#0.033333 + self._seed() + observation_high = (self._get_observation_upper_bound()) + observation_low = (self._get_observation_lower_bound()) + action_dim = 36 + self._action_bound = 3.14 #todo: check this + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(observation_low, observation_high) + + def _close(self): + self._humanoid = None + self._pybullet_client.disconnect() + + + def _reset(self): + if (self._pybullet_client==None): + if self._is_render: + self._pybullet_client = bullet_client.BulletClient( + connection_mode=pybullet.GUI) + else: + self._pybullet_client = bullet_client.BulletClient() + self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath()) + self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1) + self._motion=MotionCaptureData() + motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt" + self._motion.Load(motionPath) + self._pybullet_client.configureDebugVisualizer( + self._pybullet_client.COV_ENABLE_RENDERING, 0) + self._pybullet_client.resetSimulation() + self._pybullet_client.setGravity(0,-9.8,0) + y2zOrn = self._pybullet_client.getQuaternionFromEuler([-1.57,0,0]) + self._ground_id = self._pybullet_client.loadURDF( + "%s/plane.urdf" % self._urdf_root, [0,0,0], y2zOrn) + #self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8]) + #self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,self._ground_id) + shift=[0,0,0] + self._humanoid = Humanoid(self._pybullet_client,self._motion,shift) + + self._humanoid.Reset() + simTime = random.randint(0,self._motion.NumFrames()-2) + self._humanoid.SetSimTime(simTime) + pose = self._humanoid.InitializePoseFromMotionData() + self._humanoid.ApplyPose(pose, True, True, self._humanoid._humanoid,self._pybullet_client) + + self._env_step_counter = 0 + self._objectives = [] + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]) + self._pybullet_client.configureDebugVisualizer( + self._pybullet_client.COV_ENABLE_RENDERING, 1) + return self._get_observation() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: A list of desired motor angles for eight motors. + + Returns: + observations: The angles, velocities and torques of all motors. + reward: The reward for the current state-action pair. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + self._last_base_position = self._humanoid.GetBasePosition() + + if self._is_render: + # Sleep, otherwise the computation takes less time than real time, + # which will make the visualization like a fast-forward video. + #time_spent = time.time() - self._last_frame_time + #self._last_frame_time = time.time() + #time_to_sleep = self._control_time_step - time_spent + #if time_to_sleep > 0: + # time.sleep(time_to_sleep) + base_pos = self._humanoid.GetBasePosition() + # Keep the previous orientation of the camera set by the user. + [yaw, pitch, + dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11] + self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch, + base_pos) + + + self._humanoid.ApplyAction(action) + for s in range (8): + #print("step:",s) + self._pybullet_client.stepSimulation() + reward = self._reward() + done = self._termination() + self._env_step_counter += 1 + return np.array(self._get_observation()), reward, done, {} + + def _render(self, mode="rgb_array", close=False): + if mode == "human": + self._is_render = True + if mode != "rgb_array": + return np.array([]) + base_pos = self._humanoid.GetBasePosition() + view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=1) + proj_matrix = self._pybullet_client.computeProjectionMatrixFOV( + fov=60, + aspect=float(RENDER_WIDTH) / RENDER_HEIGHT, + nearVal=0.1, + farVal=100.0) + (_, _, px, _, _) = self._pybullet_client.getCameraImage( + width=RENDER_WIDTH, + height=RENDER_HEIGHT, + renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL, + viewMatrix=view_matrix, + projectionMatrix=proj_matrix) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + + def _termination(self): + if (self._humanoid): + term = self._humanoid.Terminates() + return term + return False + + def _reward(self): + reward = 0 + if (self._humanoid): + reward = self._humanoid.GetReward() + return reward + + def get_objectives(self): + return self._objectives + + @property + def objective_weights(self): + """Accessor for the weights for all the objectives. + + Returns: + List of floating points that corresponds to weights for the objectives in + the order that objectives are stored. + """ + return self._objective_weights + + def _get_observation(self): + """Get observation of this environment. + """ + + observation = [] + if (self._humanoid): + observation = self._humanoid.GetState() + else: + observation = [0]*197 + + self._observation = observation + return self._observation + + + def _get_observation_upper_bound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See GetObservation() for the details + of each element of an observation. + """ + upper_bound = np.zeros(self._get_observation_dimension()) + upper_bound[0] = 10 #height + upper_bound[1:107] = math.pi # Joint angle. + upper_bound[107:197] = 10 #joint velocity, check it + return upper_bound + + def _get_observation_lower_bound(self): + """Get the lower bound of the observation.""" + return -self._get_observation_upper_bound() + + def _get_observation_dimension(self): + """Get the length of the observation list. + + Returns: + The length of the observation list. + """ + return len(self._get_observation()) + + def configure(self, args): + pass + + if parse_version(gym.__version__)>=parse_version('0.9.6'): + close = _close + render = _render + reset = _reset + seed = _seed + step = _step + + + @property + def pybullet_client(self): + return self._pybullet_client + + @property + def ground_id(self): + return self._ground_id + + @ground_id.setter + def ground_id(self, new_ground_id): + self._ground_id = new_ground_id + + @property + def env_step_counter(self): + return self._env_step_counter + + diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py similarity index 91% rename from examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py rename to examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py index 2c634e018..c4eacbc1c 100644 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py @@ -3,9 +3,9 @@ currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfram parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) -from pybullet_envs.mimic.humanoid import Humanoid +from pybullet_envs.deep_mimic.humanoid import Humanoid from pybullet_utils.bullet_client import BulletClient -from pybullet_envs.mimic.motion_capture_data import MotionCaptureData +from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData import pybullet_data import pybullet import time @@ -72,6 +72,9 @@ while (1): Reset(humanoid) state = humanoid.GetState() + print("len(state)=",len(state)) + print("state=", state) + action = [0]*36 humanoid.ApplyAction(action) for s in range (8): diff --git a/examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py similarity index 100% rename from examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py rename to examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py