diff --git a/data/plane.urdf b/data/plane.urdf index ad10aeedf..1b3d09682 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -2,7 +2,7 @@ - + diff --git a/data/quadruped/LOG00076.TXT b/data/quadruped/LOG00076.TXT new file mode 100644 index 000000000..c1912b4e0 Binary files /dev/null and b/data/quadruped/LOG00076.TXT differ diff --git a/data/quadruped/minitaur.urdf b/data/quadruped/minitaur.urdf new file mode 100644 index 000000000..c483bd4b4 --- /dev/null +++ b/data/quadruped/minitaur.urdf @@ -0,0 +1,913 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/quadruped/t-motor.jpg b/data/quadruped/t-motor.jpg new file mode 100644 index 000000000..dbd5d1a6e Binary files /dev/null and b/data/quadruped/t-motor.jpg differ diff --git a/data/quadruped/tmotor.blend b/data/quadruped/tmotor.blend new file mode 100644 index 000000000..ba06c0075 Binary files /dev/null and b/data/quadruped/tmotor.blend differ diff --git a/data/quadruped/tmotor3.mtl b/data/quadruped/tmotor3.mtl new file mode 100644 index 000000000..af216a1cc --- /dev/null +++ b/data/quadruped/tmotor3.mtl @@ -0,0 +1,19 @@ +# Blender MTL File: 'tmotor.blend' +# Material Count: 2 + +newmtl None +Ns 0 +Ka 0.000000 0.000000 0.000000 +Kd 0.8 0.8 0.8 +Ks 0.8 0.8 0.8 +d 1 +illum 2 +map_Kd t-motor.jpg + +newmtl None_NONE +Ns 0 +Ka 0.000000 0.000000 0.000000 +Kd 0.8 0.8 0.8 +Ks 0.8 0.8 0.8 +d 1 +illum 2 diff --git a/data/quadruped/tmotor3.obj b/data/quadruped/tmotor3.obj new file mode 100644 index 000000000..366398a4b --- /dev/null +++ b/data/quadruped/tmotor3.obj @@ -0,0 +1,325 @@ +# Blender v2.78 (sub 0) OBJ File: 'tmotor.blend' +# www.blender.org +mtllib tmotor3.mtl +o Cylinder +v 0.000000 0.043000 -0.006500 +v 0.000000 0.043000 0.006500 +v 0.008389 0.042174 -0.006500 +v 0.008389 0.042174 0.006500 +v 0.016455 0.039727 -0.006500 +v 0.016455 0.039727 0.006500 +v 0.023890 0.035753 -0.006500 +v 0.023890 0.035753 0.006500 +v 0.030406 0.030406 -0.006500 +v 0.030406 0.030406 0.006500 +v 0.035753 0.023890 -0.006500 +v 0.035753 0.023890 0.006500 +v 0.039727 0.016455 -0.006500 +v 0.039727 0.016455 0.006500 +v 0.042174 0.008389 -0.006500 +v 0.042174 0.008389 0.006500 +v 0.043000 0.000000 -0.006500 +v 0.043000 0.000000 0.006500 +v 0.042174 -0.008389 -0.006500 +v 0.042174 -0.008389 0.006500 +v 0.039727 -0.016455 -0.006500 +v 0.039727 -0.016455 0.006500 +v 0.035753 -0.023890 -0.006500 +v 0.035753 -0.023890 0.006500 +v 0.030406 -0.030406 -0.006500 +v 0.030406 -0.030406 0.006500 +v 0.023890 -0.035753 -0.006500 +v 0.023890 -0.035753 0.006500 +v 0.016455 -0.039727 -0.006500 +v 0.016455 -0.039727 0.006500 +v 0.008389 -0.042174 -0.006500 +v 0.008389 -0.042174 0.006500 +v -0.000000 -0.043000 -0.006500 +v -0.000000 -0.043000 0.006500 +v -0.008389 -0.042174 -0.006500 +v -0.008389 -0.042174 0.006500 +v -0.016455 -0.039727 -0.006500 +v -0.016455 -0.039727 0.006500 +v -0.023890 -0.035753 -0.006500 +v -0.023890 -0.035753 0.006500 +v -0.030406 -0.030406 -0.006500 +v -0.030406 -0.030406 0.006500 +v -0.035753 -0.023889 -0.006500 +v -0.035753 -0.023889 0.006500 +v -0.039727 -0.016455 -0.006500 +v -0.039727 -0.016455 0.006500 +v -0.042174 -0.008389 -0.006500 +v -0.042174 -0.008389 0.006500 +v -0.043000 0.000000 -0.006500 +v -0.043000 0.000000 0.006500 +v -0.042174 0.008389 -0.006500 +v -0.042174 0.008389 0.006500 +v -0.039727 0.016455 -0.006500 +v -0.039727 0.016455 0.006500 +v -0.035753 0.023890 -0.006500 +v -0.035753 0.023890 0.006500 +v -0.030406 0.030406 -0.006500 +v -0.030406 0.030406 0.006500 +v -0.023889 0.035753 -0.006500 +v -0.023889 0.035753 0.006500 +v -0.016455 0.039727 -0.006500 +v -0.016455 0.039727 0.006500 +v -0.008389 0.042174 -0.006500 +v -0.008389 0.042174 0.006500 +vt 0.6520 0.1657 +vt 0.8624 0.3762 +vt 0.6520 0.8843 +vt 0.5790 0.9064 +vt 0.3543 0.8843 +vt 0.5031 0.9139 +vt 0.4273 0.9064 +vt 0.2871 0.8484 +vt 0.2281 0.8000 +vt 0.1798 0.7411 +vt 0.1438 0.6738 +vt 0.1217 0.6009 +vt 0.1142 0.5250 +vt 0.1217 0.4491 +vt 0.1438 0.3762 +vt 0.1798 0.3089 +vt 0.2281 0.2500 +vt 0.2871 0.2016 +vt 0.3543 0.1657 +vt 0.4273 0.1436 +vt 0.5031 0.1361 +vt 0.5790 0.1436 +vt 0.7192 0.2016 +vt 0.7781 0.2500 +vt 0.8265 0.3089 +vt 0.8846 0.4491 +vt 0.8920 0.5250 +vt 0.8846 0.6009 +vt 0.8624 0.6738 +vt 0.8265 0.7411 +vt 0.7781 0.8000 +vt 0.7192 0.8484 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vt 0.0000 0.0000 +vn 0.0000 0.0000 1.0000 +vn 0.0980 0.9952 0.0000 +vn 0.2903 0.9569 0.0000 +vn 0.4714 0.8819 0.0000 +vn 0.6344 0.7730 0.0000 +vn 0.7730 0.6344 0.0000 +vn 0.8819 0.4714 0.0000 +vn 0.9569 0.2903 0.0000 +vn 0.9952 0.0980 0.0000 +vn 0.9952 -0.0980 0.0000 +vn 0.9569 -0.2903 0.0000 +vn 0.8819 -0.4714 0.0000 +vn 0.7730 -0.6344 0.0000 +vn 0.6344 -0.7730 0.0000 +vn 0.4714 -0.8819 0.0000 +vn 0.2903 -0.9569 0.0000 +vn 0.0980 -0.9952 0.0000 +vn -0.0980 -0.9952 0.0000 +vn -0.2903 -0.9569 0.0000 +vn -0.4714 -0.8819 0.0000 +vn -0.6344 -0.7730 0.0000 +vn -0.7730 -0.6344 0.0000 +vn -0.8819 -0.4714 0.0000 +vn -0.9569 -0.2903 0.0000 +vn -0.9952 -0.0980 0.0000 +vn -0.9952 0.0980 0.0000 +vn -0.9569 0.2903 0.0000 +vn -0.8819 0.4714 0.0000 +vn -0.7730 0.6344 0.0000 +vn -0.6344 0.7730 0.0000 +vn -0.4714 0.8819 0.0000 +vn -0.2903 0.9569 0.0000 +vn -0.0980 0.9952 0.0000 +vn 0.0000 0.0000 -1.0000 +usemtl None +s off +f 30/1/1 22/2/1 6/3/1 +f 6/3/1 4/4/1 62/5/1 +f 2/6/1 64/7/1 62/5/1 +f 62/5/1 60/8/1 58/9/1 +f 58/9/1 56/10/1 54/11/1 +f 54/11/1 52/12/1 50/13/1 +f 50/13/1 48/14/1 54/11/1 +f 46/15/1 44/16/1 42/17/1 +f 42/17/1 40/18/1 38/19/1 +f 38/19/1 36/20/1 34/21/1 +f 34/21/1 32/22/1 38/19/1 +f 30/1/1 28/23/1 26/24/1 +f 26/24/1 24/25/1 22/2/1 +f 22/2/1 20/26/1 18/27/1 +f 18/27/1 16/28/1 22/2/1 +f 14/29/1 12/30/1 10/31/1 +f 10/31/1 8/32/1 6/3/1 +f 4/4/1 2/6/1 62/5/1 +f 62/5/1 58/9/1 6/3/1 +f 54/11/1 48/14/1 46/15/1 +f 46/15/1 42/17/1 54/11/1 +f 38/19/1 32/22/1 30/1/1 +f 30/1/1 26/24/1 22/2/1 +f 22/2/1 16/28/1 14/29/1 +f 14/29/1 10/31/1 22/2/1 +f 6/3/1 58/9/1 54/11/1 +f 54/11/1 42/17/1 38/19/1 +f 38/19/1 30/1/1 6/3/1 +f 22/2/1 10/31/1 6/3/1 +f 6/3/1 54/11/1 38/19/1 +usemtl None +f 2/33/2 3/34/2 1/35/2 +f 4/36/3 5/37/3 3/34/3 +f 6/38/4 7/39/4 5/37/4 +f 8/40/5 9/41/5 7/39/5 +f 10/42/6 11/43/6 9/41/6 +f 12/44/7 13/45/7 11/43/7 +f 14/46/8 15/47/8 13/45/8 +f 16/48/9 17/49/9 15/47/9 +f 18/50/10 19/51/10 17/49/10 +f 20/52/11 21/53/11 19/51/11 +f 22/54/12 23/55/12 21/53/12 +f 24/56/13 25/57/13 23/55/13 +f 26/58/14 27/59/14 25/57/14 +f 28/60/15 29/61/15 27/59/15 +f 30/62/16 31/63/16 29/61/16 +f 32/64/17 33/65/17 31/63/17 +f 34/66/18 35/67/18 33/65/18 +f 36/68/19 37/69/19 35/67/19 +f 38/70/20 39/71/20 37/69/20 +f 40/72/21 41/73/21 39/71/21 +f 42/74/22 43/75/22 41/73/22 +f 44/76/23 45/77/23 43/75/23 +f 46/78/24 47/79/24 45/77/24 +f 48/80/25 49/81/25 47/79/25 +f 50/82/26 51/83/26 49/81/26 +f 52/84/27 53/85/27 51/83/27 +f 54/86/28 55/87/28 53/85/28 +f 56/88/29 57/89/29 55/87/29 +f 58/90/30 59/91/30 57/89/30 +f 60/92/31 61/93/31 59/91/31 +f 62/94/32 63/95/32 61/93/32 +f 64/96/33 1/35/33 63/95/33 +f 31/63/34 55/87/34 63/95/34 +f 2/33/2 4/36/2 3/34/2 +f 4/36/3 6/38/3 5/37/3 +f 6/38/4 8/40/4 7/39/4 +f 8/40/5 10/42/5 9/41/5 +f 10/42/6 12/44/6 11/43/6 +f 12/44/7 14/46/7 13/45/7 +f 14/46/8 16/48/8 15/47/8 +f 16/48/9 18/50/9 17/49/9 +f 18/50/10 20/52/10 19/51/10 +f 20/52/11 22/54/11 21/53/11 +f 22/54/12 24/56/12 23/55/12 +f 24/56/13 26/58/13 25/57/13 +f 26/58/14 28/60/14 27/59/14 +f 28/60/15 30/62/15 29/61/15 +f 30/62/16 32/64/16 31/63/16 +f 32/64/17 34/66/17 33/65/17 +f 34/66/18 36/68/18 35/67/18 +f 36/68/19 38/70/19 37/69/19 +f 38/70/20 40/72/20 39/71/20 +f 40/72/21 42/74/21 41/73/21 +f 42/74/22 44/76/22 43/75/22 +f 44/76/23 46/78/23 45/77/23 +f 46/78/24 48/80/24 47/79/24 +f 48/80/25 50/82/25 49/81/25 +f 50/82/26 52/84/26 51/83/26 +f 52/84/27 54/86/27 53/85/27 +f 54/86/28 56/88/28 55/87/28 +f 56/88/29 58/90/29 57/89/29 +f 58/90/30 60/92/30 59/91/30 +f 60/92/31 62/94/31 61/93/31 +f 62/94/32 64/96/32 63/95/32 +f 64/96/33 2/33/33 1/35/33 +f 63/95/34 1/35/34 3/34/34 +f 3/34/34 5/37/34 7/39/34 +f 7/39/34 9/41/34 15/47/34 +f 11/43/34 13/45/34 15/47/34 +f 15/47/34 17/49/34 19/51/34 +f 19/51/34 21/53/34 23/55/34 +f 23/55/34 25/57/34 31/63/34 +f 27/59/34 29/61/34 31/63/34 +f 31/63/34 33/65/34 35/67/34 +f 35/67/34 37/69/34 31/63/34 +f 39/71/34 41/73/34 47/79/34 +f 43/75/34 45/77/34 47/79/34 +f 47/79/34 49/81/34 51/83/34 +f 51/83/34 53/85/34 55/87/34 +f 55/87/34 57/89/34 63/95/34 +f 59/91/34 61/93/34 63/95/34 +f 63/95/34 3/34/34 15/47/34 +f 9/41/34 11/43/34 15/47/34 +f 15/47/34 19/51/34 31/63/34 +f 25/57/34 27/59/34 31/63/34 +f 31/63/34 37/69/34 39/71/34 +f 41/73/34 43/75/34 47/79/34 +f 47/79/34 51/83/34 55/87/34 +f 57/89/34 59/91/34 63/95/34 +f 3/34/34 7/39/34 15/47/34 +f 19/51/34 23/55/34 31/63/34 +f 31/63/34 39/71/34 47/79/34 +f 47/79/34 55/87/34 31/63/34 +f 63/95/34 15/47/34 31/63/34 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8bf8467b4..26b56aee5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -512,7 +512,8 @@ struct MinitaurStateLogger : public InternalStateLogger MinitaurLogRecord logData; //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo' - btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1}; + btScalar motorDir[8] = {1, 1, 1, 1, 1, 1, 1, 1}; + btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation(); btMatrix3x3 mat(orn); @@ -568,12 +569,14 @@ struct GenericRobotStateLogger : public InternalStateLogger btMultiBodyDynamicsWorld* m_dynamicsWorld; btAlignedObjectArray m_bodyIdList; bool m_filterObjectUniqueId; - - GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld) + int m_maxLogDof; + + GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof) :m_loggingTimeStamp(0), m_logFileHandle(0), m_dynamicsWorld(dynamicsWorld), - m_filterObjectUniqueId(false) + m_filterObjectUniqueId(false), + m_maxLogDof(maxLogDof) { m_loggingType = STATE_LOGGING_GENERIC_ROBOT; @@ -595,32 +598,24 @@ struct GenericRobotStateLogger : public InternalStateLogger structNames.push_back("omegaY"); structNames.push_back("omegaZ"); structNames.push_back("qNum"); - structNames.push_back("q0"); - structNames.push_back("q1"); - structNames.push_back("q2"); - structNames.push_back("q3"); - structNames.push_back("q4"); - structNames.push_back("q5"); - structNames.push_back("q6"); - structNames.push_back("q7"); - structNames.push_back("q8"); - structNames.push_back("q9"); - structNames.push_back("q10"); - structNames.push_back("q11"); - structNames.push_back("u0"); - structNames.push_back("u1"); - structNames.push_back("u2"); - structNames.push_back("u3"); - structNames.push_back("u4"); - structNames.push_back("u5"); - structNames.push_back("u6"); - structNames.push_back("u7"); - structNames.push_back("u8"); - structNames.push_back("u9"); - structNames.push_back("u10"); - structNames.push_back("u11"); + + m_structTypes = "IfIfffffffffffffI"; + + for (int i=0;im_dynamicsWorld); + GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof); if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) { diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py index 5e4eade4f..10a10cb45 100644 --- a/examples/pybullet/quadruped.py +++ b/examples/pybullet/quadruped.py @@ -2,32 +2,35 @@ import pybullet as p import time import math -useRealTime = 1 +useRealTime = 0 fixedTimeStep = 0.001 speed = 10 -amplitude = 1.3 +amplitude = 0.8 jump_amp = 0.5 maxForce = 3.5 -kp = .6 -kd = 1 +kneeFrictionForce = 0.00 +kp = .05 +kd = .5 physId = p.connect(p.SHARED_MEMORY) if (physId<0): p.connect(p.GUI) - -p.loadURDF("plane.urdf") -p.setGravity(0,0,-10) +#p.resetSimulation() +p.loadURDF("plane.urdf",0,0,0.1) +#p.loadSDF("kitchens/1.sdf") +p.setGravity(0,0,0) p.setTimeStep(fixedTimeStep) -p.setRealTimeSimulation(1) -quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,0.3) +p.setRealTimeSimulation(0) +quadruped = p.loadURDF("quadruped/minitaur.urdf",[0,0,0.2],useFixedBase=False) nJoints = p.getNumJoints(quadruped) jointNameToId = {} for i in range(nJoints): - jointInfo = p.getJointInfo(quadruped, i) - jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + jointInfo = p.getJointInfo(quadruped, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] @@ -56,93 +59,70 @@ knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] #p.getNumJoints(1) -#right front leg -p.resetJointState(quadruped,0,1.57) -p.resetJointState(quadruped,2,-2.2) -p.resetJointState(quadruped,3,-1.57) -p.resetJointState(quadruped,5,2.2) -p.createConstraint(quadruped,2,quadruped,5,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) +p.resetJointState(quadruped,motor_front_rightR_joint,1.57) +p.resetJointState(quadruped,knee_front_rightR_link,-2.2) +p.resetJointState(quadruped,motor_front_rightL_joint,1.57) +p.resetJointState(quadruped,knee_front_rightL_link,-2.2) +p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0) +p.resetJointState(quadruped,motor_front_leftR_joint,-1.57) +p.resetJointState(quadruped,knee_front_leftR_link,2.2) +p.resetJointState(quadruped,motor_front_leftL_joint,-1.57) +p.resetJointState(quadruped,knee_front_leftL_link,2.2) +p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -#left front leg -p.resetJointState(quadruped,6,1.57) -p.resetJointState(quadruped,8,-2.2) -p.resetJointState(quadruped,9,-1.57) -p.resetJointState(quadruped,11,2.2) -p.createConstraint(quadruped,8,quadruped,11,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) +p.resetJointState(quadruped,motor_back_rightR_joint,1.57) +p.resetJointState(quadruped,knee_back_rightR_link,-2.2) +p.resetJointState(quadruped,motor_back_rightL_joint,1.57) +p.resetJointState(quadruped,knee_back_rightL_link,-2.2) +p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,7,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,8,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,10,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,11,p.VELOCITY_CONTROL,0,0) - - -#right back leg -p.resetJointState(quadruped,12,1.57) -p.resetJointState(quadruped,14,-2.2) -p.resetJointState(quadruped,15,-1.57) -p.resetJointState(quadruped,17,2.2) -p.createConstraint(quadruped,14,quadruped,17,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) - -p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0) - -#left back leg -p.resetJointState(quadruped,18,1.57) -p.resetJointState(quadruped,20,-2.2) -p.resetJointState(quadruped,21,-1.57) -p.resetJointState(quadruped,23,2.2) -p.createConstraint(quadruped,20,quadruped,23,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) - -p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0) - - -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce) -p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) +p.resetJointState(quadruped,motor_back_leftR_joint,-1.57) +p.resetJointState(quadruped,knee_back_leftR_link,2.2) +p.resetJointState(quadruped,motor_back_leftL_joint,-1.57) +p.resetJointState(quadruped,knee_back_leftL_link,2.2) +p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.setJointMotorControl(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,-1.57,maxForce) +p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,-1.57,maxForce) +p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -#stand still -p.setRealTimeSimulation(useRealTime) +motordir=[-1,-1,-1,-1,1,1,1,1] +legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] + +#use the Minitaur leg numbering +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) -t=0.0 -ref_time = time.time() -t_end = t + 4 -while t < t_end: - if (useRealTime==0): - t = t+fixedTimeStep - p.stepSimulation() - else: - t = time.time()-ref_time - p.setGravity(0,0,-10) +p.stepSimulation() + + +print("quadruped Id = ") +print(quadruped) +p.saveWorld("quadru.py") +logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped]) p.setGravity(0,0,-10) - + +#stand still +p.setRealTimeSimulation(1) + #jump t = 0.0 t_end = t + 10 @@ -163,49 +143,17 @@ while t < t_end: p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce) + + target = math.sin(t*speed)*jump_amp+1.57; + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce) if (useRealTime==0): p.stepSimulation() -#hop forward -t_end = 20 -i=0 -while t < t_end: - if (useRealTime): - t = time.time()-ref_time - else: - t = t+fixedTimeStep - - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed+3.14)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed+3.14)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - - if (useRealTime==0): - p.stepSimulation() - -#walk -t_end = 100 -i=0 -while t < t_end: - if (useRealTime): - t = time.time()-ref_time - else: - t = t+fixedTimeStep - - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3+0.5*3.14)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3+3.14)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3+1.5*3.14)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce) - p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce) - - - p.stepSimulation() -p.setRealTimeSimulation(1) diff --git a/examples/pybullet/quadruped_playback.py b/examples/pybullet/quadruped_playback.py new file mode 100644 index 000000000..7aaefda2e --- /dev/null +++ b/examples/pybullet/quadruped_playback.py @@ -0,0 +1,126 @@ +import pybullet as p +import time +import math +from datetime import datetime +from numpy import * +from pylab import * +import struct +import sys +import os, fnmatch +import argparse +from time import sleep + +def readLogFile(filename, verbose = True): + f = open(filename, 'rb') + + print('Opened'), + print(filename) + + keys = f.readline().decode('utf8').rstrip('\n').split(',') + fmt = f.readline().decode('utf8').rstrip('\n') + + # The byte number of one record + sz = struct.calcsize(fmt) + # The type number of one record + ncols = len(fmt) + + if verbose: + print('Keys:'), + print(keys) + print('Format:'), + print(fmt) + print('Size:'), + print(sz) + print('Columns:'), + print(ncols) + + # Read data + wholeFile = f.read() + # split by alignment word + chunks = wholeFile.split(b'\xaa\xbb') + print ("num chunks") + print (len(chunks)) + + log = list() + for chunk in chunks: + if len(chunk) == sz: + values = struct.unpack(fmt, chunk) + record = list() + for i in range(ncols): + record.append(values[i]) + log.append(record) + + return log + +clid = p.connect(p.SHARED_MEMORY) + +log = readLogFile("LOG00076.TXT"); + +recordNum = len(log) +print('record num:'), +print(recordNum) +itemNum = len(log[0]) +print('item num:'), +print(itemNum) + +useRealTime = 0 +fixedTimeStep = 0.001 +speed = 10 +amplitude = 0.8 +jump_amp = 0.5 +maxForce = 3.5 +kp = .05 +kd = .5 + + + +quadruped = 1 +nJoints = p.getNumJoints(quadruped) +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(quadruped, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + +motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] +hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] +knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] +motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] +motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] +knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] +motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] +hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] +knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] +motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint'] +motor_front_leftL_link = jointNameToId['motor_front_leftL_link'] +knee_front_leftL_link = jointNameToId['knee_front_leftL_link'] +motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint'] +hip_rightR_link = jointNameToId['hip_rightR_link'] +knee_back_rightR_link = jointNameToId['knee_back_rightR_link'] +motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint'] +motor_back_rightL_link = jointNameToId['motor_back_rightL_link'] +knee_back_rightL_link = jointNameToId['knee_back_rightL_link'] +motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint'] +hip_leftR_link = jointNameToId['hip_leftR_link'] +knee_back_leftR_link = jointNameToId['knee_back_leftR_link'] +motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] +motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] +knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] + +motorDir = [1, 1, 1, 1, 1, 1, 1, 1]; +legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] + + +for record in log: + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce) + p.setGravity(0.000000,0.000000,-10.000000) + p.stepSimulation() + p.stepSimulation() + sleep(0.01) + \ No newline at end of file diff --git a/examples/pybullet/quadruped_setup_playback.py b/examples/pybullet/quadruped_setup_playback.py new file mode 100644 index 000000000..a2b041888 --- /dev/null +++ b/examples/pybullet/quadruped_setup_playback.py @@ -0,0 +1,21 @@ +import pybullet as p +p.connect(p.SHARED_MEMORY) +objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)] +ob = objects[0] +jointPositions=[ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ] +for ji in range (p.getNumJoints(ob)): + p.resetJointState(ob,ji,jointPositions[ji]) + p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0) + +cid0 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid0,maxForce=500.000000) +cid1 = p.createConstraint(1,16,1,19,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid1,maxForce=500.000000) +cid2 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid2,maxForce=500.000000) +cid3 = p.createConstraint(1,22,1,25,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid3,maxForce=500.000000) +p.setGravity(0.000000,0.000000,0.000000) +p.stepSimulation() +p.disconnect()