From 224eceb75bf4db922e89fac1a84f251631886f6c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 3 Sep 2020 21:16:39 -0700 Subject: [PATCH] add test script for spirit40 --- .../gym/pybullet_data/quadruped/spirit40.py | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_data/quadruped/spirit40.py diff --git a/examples/pybullet/gym/pybullet_data/quadruped/spirit40.py b/examples/pybullet/gym/pybullet_data/quadruped/spirit40.py new file mode 100644 index 000000000..cb1d5ce9c --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/quadruped/spirit40.py @@ -0,0 +1,63 @@ +from __future__ import print_function +import time +import numpy as np +import argparse + + +import pybullet as p +import time +import pybullet_data as pd +p.connect(p.GUI) +p.setAdditionalSearchPath(pd.getDataPath()) +dt = 1./240. + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +p.loadURDF("plane.urdf") +robot = p.loadURDF("quadruped/spirit40.urdf",[0,0,.5], useFixedBase=False) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +p.setGravity(0,0,-9.8) + + +#j= (0, b'8', 0, 7, 6, 1, 0.0, 0.0, -0.707, 0.707, 40.0, 30.0, b'hip0', (1.0, 0.0, 0.0), (0.2263, 0.07, 0.0), (0.0, 0.0, 0.0, 1.0), -1) +#j= (1, b'0', 0, 8, 7, 1, 0.0, 0.0, -6.28318530718, 6.28318530718, 40.0, 30.0, b'upper0', (0.0, -1.0, 0.0), (0.0, 0.10098, 0.0), (0.0, 0.0, 0.0, 1.0), 0) +#j= (2, b'1', 0, 9, 8, 1, 0.0, 0.0, 0.0, 3.14159265359, 40.0, 30.0, b'lower0', (0.0, 1.0, 0.0), (-0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 1) +#j= (3, b'jtoe0', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'toe0', (0.0, 0.0, 0.0), (0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 2) +#j= (4, b'9', 0, 10, 9, 1, 0.0, 0.0, -0.707, 0.707, 40.0, 30.0, b'hip1', (1.0, 0.0, 0.0), (-0.2263, 0.07, 0.0), (0.0, 0.0, 0.0, 1.0), -1) +#j= (5, b'2', 0, 11, 10, 1, 0.0, 0.0, -6.28318530718, 6.28318530718, 40.0, 30.0, b'upper1', (0.0, -1.0, 0.0), (0.0, 0.10098, 0.0), (0.0, 0.0, 0.0, 1.0), 4) +#j= (6, b'3', 0, 12, 11, 1, 0.0, 0.0, 0.0, 3.14159265359, 40.0, 30.0, b'lower1', (0.0, 1.0, 0.0), (-0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 5) +#j= (7, b'jtoe1', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'toe1', (0.0, 0.0, 0.0), (0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 6) +#j= (8, b'10', 0, 13, 12, 1, 0.0, 0.0, -0.707, 0.707, 40.0, 30.0, b'hip2', (1.0, 0.0, 0.0), (0.2263, -0.07, 0.0), (0.0, 0.0, 0.0, 1.0), -1) +#j= (9, b'4', 0, 14, 13, 1, 0.0, 0.0, -6.28318530718, 6.28318530718, 40.0, 30.0, b'upper2', (0.0, -1.0, 0.0), (0.0, -0.10098, 0.0), (0.0, 0.0, 0.0, 1.0), 8) +#j= (10, b'5', 0, 15, 14, 1, 0.0, 0.0, 0.0, 3.14159265359, 40.0, 30.0, b'lower2', (0.0, 1.0, 0.0), (-0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 9) +#j= (11, b'jtoe2', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'toe2', (0.0, 0.0, 0.0), (0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 10) +#j= (12, b'11', 0, 16, 15, 1, 0.0, 0.0, -0.707, 0.707, 40.0, 30.0, b'hip3', (1.0, 0.0, 0.0), (-0.2263, -0.07, 0.0), (0.0, 0.0, 0.0, 1.0), -1) +#j= (13, b'6', 0, 17, 16, 1, 0.0, 0.0, -6.28318530718, 6.28318530718, 40.0, 30.0, b'upper3', (0.0, -1.0, 0.0), (0.0, -0.10098, 0.0), (0.0, 0.0, 0.0, 1.0), 12) +#j= (14, b'7', 0, 18, 17, 1, 0.0, 0.0, 0.0, 3.14159265359, 40.0, 30.0, b'lower3', (0.0, 1.0, 0.0), (-0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 13) +#j= (15, b'jtoe3', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'toe3', (0.0, 0.0, 0.0), (0.206, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 14) + +toes=[3,7,11,15] +revolute_joints=[1,2, 5,6, 9,10, 13,14, 0, 4, 8, 12] + +joint_positions = [0.11321902, 0.1833849, 0.11935401, 0.1723926, 0.13162422, 0.2013433, + 0.09736967, 0.16855788, -0.08001328, -0.140342, 0.0006392, 0.11989117] + + +joint_positions = [0.74833727, 1.4664032, 0.76712584, 1.4589894, 0.74642015, 1.4782903, + 0.7488482, 1.4464638 , -0.01239824 ,-0.02952504, 0.02939725, 0.05764484] + +for j in range (p.getNumJoints(robot)): + print("j=", p.getJointInfo(robot,j)) + +for j in range (12): + joint_index = revolute_joints[j] + print("revolute_joint index=", joint_index) + p.resetJointState(robot, joint_index, joint_positions[j]) + p.setJointMotorControl(robot,joint_index,p.POSITION_CONTROL, joint_positions[j]) + +count = 0 +while p.isConnected(): + count+=1 + p.stepSimulation() + time.sleep(dt) +print("sitting!") +