p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
# p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 1)
# load urdf file path
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# load urdf and set gravity
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 0.3]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, math.pi / 2])
# cubeStartOrientation = transformations.quaternion_from_euler(math.pi / 2, 0, 0, 'szyx')
p.setAdditionalSearchPath(str(pathlib.Path(__file__).parent.absolute()))
env = OpenDynamicQuadruped(pybullet_client=p)
mocapData = motion_capture_data.MotionCaptureData()
motionPath = pybullet_data.getDataPath() + "/data/motions/laikago_walk.txt"
mocapData.Load(motionPath)
print("mocapData.NumFrames=", mocapData.NumFrames())
print("mocapData.KeyFrameDuraction=", mocapData.KeyFrameDuraction())
print("mocapData.getCycleTime=", mocapData.getCycleTime())
print("mocapData.computeCycleOffset=", mocapData.computeCycleOffset())
qpi = quadrupedPoseInterpolator.QuadrupedPoseInterpolator()
stablePD = pd_controller_stable.PDControllerStable(p)
jointDirections = [1, 1, -1, 1, -1, 1, 1, 1, -1, -1, 1, -1]
# jointDirections = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# jointDirections = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
def reset(self):
if not self._isInitialized:
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=p1.GUI)
#disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_GUI, 0)
else:
self._pybullet_client = bullet_client.BulletClient()
self._pybullet_client.setAdditionalSearchPath(
pybullet_data.getDataPath())
z2y = self._pybullet_client.getQuaternionFromEuler(
[-math.pi * 0.5, 0, 0])
self._planeId = self._pybullet_client.loadURDF(
"plane_implicit.urdf", [0, 0, 0],
z2y,
useMaximalCoordinates=True)
#print("planeId=",self._planeId)
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
self._pybullet_client.setGravity(0, -9.8, 0)
self._pybullet_client.setPhysicsEngineParameter(
numSolverIterations=10)
self._pybullet_client.changeDynamics(self._planeId,
linkIndex=-1,
lateralFriction=0.9)
self._mocapData = motion_capture_data.MotionCaptureData()
motion_file = self._arg_parser.parse_strings('motion_file')
print("motion_file=", motion_file[0])
motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath)
timeStep = 1. / 240.
useFixedBase = False
self._humanoid = humanoid_stable_pd.HumanoidStablePD(
self._pybullet_client, self._mocapData, timeStep, useFixedBase,
self._arg_parser)
self._isInitialized = True
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
selfCheck = False
if (selfCheck):
curTime = 0
while self._pybullet_client.isConnected():
self._humanoid.setSimTime(curTime)
state = self._humanoid.getState()
#print("state=",state)
pose = self._humanoid.computePose(
self._humanoid._frameFraction)
for i in range(10):
curTime += timeStep
#taus = self._humanoid.computePDForces(pose)
#self._humanoid.applyPDForces(taus)
#self._pybullet_client.stepSimulation()
time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
rnrange = 1000
rn = random.randint(0, rnrange)
startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
self.t = startTime
self._humanoid.setSimTime(startTime)
self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points?
#self._pybullet_client.stepSimulation()
self._humanoid.resetPose()
self.needs_update_time = self.t - 1 #force update