00361afea5
add minitaur quadruped playback of minitaur log files (real robot and simulated create the same log files) add improved minitaur.urdf file, see https://youtu.be/lv7lybtOzeo for a preview.
126 lines
4.9 KiB
Python
126 lines
4.9 KiB
Python
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)
|
|
|