Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
Erwin Coumans
92
examples/pybullet/kuka_with_cube.py
Normal file
92
examples/pybullet/kuka_with_cube.py
Normal file
@@ -0,0 +1,92 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
from datetime import datetime
|
||||
|
||||
#clid = p.connect(p.SHARED_MEMORY)
|
||||
p.connect(p.GUI)
|
||||
p.loadURDF("plane.urdf",[0,0,-0.3],useFixedBase=True)
|
||||
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0],useFixedBase=True)
|
||||
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
|
||||
kukaEndEffectorIndex = 6
|
||||
numJoints = p.getNumJoints(kukaId)
|
||||
if (numJoints!=7):
|
||||
exit()
|
||||
|
||||
p.loadURDF("cube.urdf",[2,2,5])
|
||||
p.loadURDF("cube.urdf",[-2,-2,5])
|
||||
p.loadURDF("cube.urdf",[2,-2,5])
|
||||
|
||||
#lower limits for null space
|
||||
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
|
||||
#upper limits for null space
|
||||
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
|
||||
#joint ranges for null space
|
||||
jr=[5.8,4,5.8,4,5.8,4,6]
|
||||
#restposes for null space
|
||||
rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
|
||||
#joint damping coefficents
|
||||
jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
|
||||
|
||||
for i in range (numJoints):
|
||||
p.resetJointState(kukaId,i,rp[i])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
t=0.
|
||||
prevPose=[0,0,0]
|
||||
prevPose1=[0,0,0]
|
||||
hasPrevPose = 0
|
||||
useNullSpace = 0
|
||||
|
||||
count = 0
|
||||
useOrientation = 1
|
||||
useSimulation = 1
|
||||
useRealTimeSimulation = 1
|
||||
p.setRealTimeSimulation(useRealTimeSimulation)
|
||||
#trailDuration is duration (in seconds) after debug lines will be removed automatically
|
||||
#use 0 for no-removal
|
||||
trailDuration = 15
|
||||
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
|
||||
|
||||
while 1:
|
||||
if (useRealTimeSimulation):
|
||||
dt = datetime.now()
|
||||
t = (dt.second/60.)*2.*math.pi
|
||||
else:
|
||||
t=t+0.1
|
||||
|
||||
if (useSimulation and useRealTimeSimulation==0):
|
||||
p.stepSimulation()
|
||||
|
||||
for i in range (1):
|
||||
pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
|
||||
#end effector points down, not up (in case useOrientation==1)
|
||||
orn = p.getQuaternionFromEuler([0,-math.pi,0])
|
||||
|
||||
if (useNullSpace==1):
|
||||
if (useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
|
||||
else:
|
||||
if (useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
|
||||
|
||||
if (useSimulation):
|
||||
for i in range (numJoints):
|
||||
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
|
||||
else:
|
||||
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
|
||||
for i in range (numJoints):
|
||||
p.resetJointState(kukaId,i,jointPoses[i])
|
||||
|
||||
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
|
||||
if (hasPrevPose):
|
||||
p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
|
||||
p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
|
||||
prevPose=pos
|
||||
prevPose1=ls[4]
|
||||
hasPrevPose = 1
|
||||
77
examples/pybullet/kuka_with_cube_playback.py
Normal file
77
examples/pybullet/kuka_with_cube_playback.py
Normal file
@@ -0,0 +1,77 @@
|
||||
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().rstrip('\n').split(',')
|
||||
fmt = f.readline().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('\xaa\xbb')
|
||||
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)
|
||||
p.connect(p.GUI)
|
||||
p.loadURDF("plane.urdf",[0,0,-0.3])
|
||||
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
|
||||
p.loadURDF("cube.urdf",[2,2,5])
|
||||
p.loadURDF("cube.urdf",[-2,-2,5])
|
||||
p.loadURDF("cube.urdf",[2,-2,5])
|
||||
|
||||
log = readLogFile("LOG0001.txt")
|
||||
|
||||
recordNum = len(log)
|
||||
itemNum = len(log[0])
|
||||
print('record num:'),
|
||||
print(recordNum)
|
||||
print('item num:'),
|
||||
print(itemNum)
|
||||
|
||||
for record in log:
|
||||
Id = record[1]
|
||||
pos = [record[2],record[3],record[4]]
|
||||
orn = [record[5],record[6],record[7],record[8]]
|
||||
p.resetBasePositionAndOrientation(Id,pos,orn)
|
||||
numJoints = record[15]
|
||||
for i in range (numJoints):
|
||||
p.resetJointState(Id,i,record[i+16])
|
||||
sleep(0.0005)
|
||||
@@ -1,14 +1,12 @@
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
|
||||
class Minitaur:
|
||||
def __init__(self):
|
||||
def __init__(self, urdfRootPath=''):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
|
||||
self.kp = 1
|
||||
self.kd = 0.1
|
||||
self.maxForce = 100
|
||||
def buildJointNameToIdDict(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
self.jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
@@ -18,13 +16,39 @@ class Minitaur:
|
||||
for i in range(100):
|
||||
p.stepSimulation()
|
||||
|
||||
def buildMotorIdList(self):
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
|
||||
|
||||
|
||||
def reset(self):
|
||||
self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3)
|
||||
self.kp = 1
|
||||
self.kd = 0.1
|
||||
self.maxForce = 3.5
|
||||
self.nMotors = 8
|
||||
self.motorIdList = []
|
||||
self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1]
|
||||
self.buildJointNameToIdDict()
|
||||
self.buildMotorIdList()
|
||||
|
||||
|
||||
def disableAllMotors(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
for i in range(nJoints):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||
|
||||
def setMotorAngleById(self, motorId, desiredAngle):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
|
||||
|
||||
def setMotorAngleByName(self, motorName, desiredAngle):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=self.jointNameToId[motorName], controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
|
||||
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
|
||||
|
||||
def resetPose(self):
|
||||
#right front leg
|
||||
@@ -73,11 +97,30 @@ class Minitaur:
|
||||
return orientation
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
self.setMotorAngleByName('motor_front_rightR_joint', motorCommands[0])
|
||||
self.setMotorAngleByName('motor_front_rightL_joint', motorCommands[1])
|
||||
self.setMotorAngleByName('motor_front_leftR_joint', motorCommands[2])
|
||||
self.setMotorAngleByName('motor_front_leftL_joint', motorCommands[3])
|
||||
self.setMotorAngleByName('motor_back_rightR_joint', motorCommands[4])
|
||||
self.setMotorAngleByName('motor_back_rightL_joint', motorCommands[5])
|
||||
self.setMotorAngleByName('motor_back_leftR_joint', motorCommands[6])
|
||||
self.setMotorAngleByName('motor_back_leftL_joint', motorCommands[7])
|
||||
motorCommandsWithDir = np.multiply(motorCommands, self.motorDir)
|
||||
for i in range(self.nMotors):
|
||||
self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i])
|
||||
|
||||
def getMotorAngles(self):
|
||||
motorAngles = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorAngles.append(jointState[0])
|
||||
motorAngles = np.multiply(motorAngles, self.motorDir)
|
||||
return motorAngles
|
||||
|
||||
def getMotorVelocities(self):
|
||||
motorVelocities = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorVelocities.append(jointState[1])
|
||||
motorVelocities = np.multiply(motorVelocities, self.motorDir)
|
||||
return motorVelocities
|
||||
|
||||
def getMotorTorques(self):
|
||||
motorTorques = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorTorques.append(jointState[3])
|
||||
motorTorques = np.multiply(motorTorques, self.motorDir)
|
||||
return motorTorques
|
||||
|
||||
99
examples/pybullet/minitaur_evaluate.py
Normal file
99
examples/pybullet/minitaur_evaluate.py
Normal file
@@ -0,0 +1,99 @@
|
||||
from minitaur import Minitaur
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
import time
|
||||
import sys
|
||||
import math
|
||||
|
||||
minitaur = None
|
||||
|
||||
evaluate_func_map = dict()
|
||||
|
||||
|
||||
def current_position():
|
||||
global minitaur
|
||||
position = minitaur.getBasePosition()
|
||||
return np.asarray(position)
|
||||
|
||||
def is_fallen():
|
||||
global minitaur
|
||||
orientation = minitaur.getBaseOrientation()
|
||||
rotMat = p.getMatrixFromQuaterion(orientation)
|
||||
localUp = rotMat[6:]
|
||||
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
|
||||
|
||||
def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
|
||||
nMotors = 8
|
||||
speed = 0.35
|
||||
for jthMotor in range(nMotors):
|
||||
joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57
|
||||
return joint_values
|
||||
|
||||
def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
|
||||
speed = 0.35
|
||||
phaseDiff = params[2]
|
||||
a0 = math.sin(i * speed) * params[0] + 1.57
|
||||
a1 = math.sin(i * speed + phaseDiff) * params[1] + 1.57
|
||||
a2 = math.sin(i * speed + params[3]) * params[0] + 1.57
|
||||
a3 = math.sin(i * speed + params[3] + phaseDiff) * params[1] + 1.57
|
||||
a4 = math.sin(i * speed + params[4] + phaseDiff) * params[1] + 1.57
|
||||
a5 = math.sin(i * speed + params[4]) * params[0] + 1.57
|
||||
a6 = math.sin(i * speed + params[5] + phaseDiff) * params[1] + 1.57
|
||||
a7 = math.sin(i * speed + params[5]) * params[0] + 1.57
|
||||
joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
|
||||
return joint_values
|
||||
|
||||
def evaluate_desired_motorAngle_hop(i, params):
|
||||
amplitude = params[0]
|
||||
speed = params[1]
|
||||
a1 = math.sin(i*speed)*amplitude+1.57
|
||||
a2 = math.sin(i*speed+3.14)*amplitude+1.57
|
||||
joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2]
|
||||
return joint_values
|
||||
|
||||
|
||||
evaluate_func_map['evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
|
||||
evaluate_func_map['evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
|
||||
evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop
|
||||
|
||||
|
||||
|
||||
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0):
|
||||
print('start evaluation')
|
||||
beforeTime = time.time()
|
||||
p.resetSimulation()
|
||||
|
||||
p.setTimeStep(timeStep)
|
||||
p.loadURDF("%s/plane.urdf" % urdfRoot)
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
global minitaur
|
||||
minitaur = Minitaur(urdfRoot)
|
||||
start_position = current_position()
|
||||
last_position = None # for tracing line
|
||||
total_energy = 0
|
||||
|
||||
for i in range(maxNumSteps):
|
||||
torques = minitaur.getMotorTorques()
|
||||
velocities = minitaur.getMotorVelocities()
|
||||
total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
|
||||
|
||||
joint_values = evaluate_func_map[evaluateFunc](i, params)
|
||||
minitaur.applyAction(joint_values)
|
||||
p.stepSimulation()
|
||||
if (is_fallen()):
|
||||
break
|
||||
|
||||
if i % 100 == 0:
|
||||
sys.stdout.write('.')
|
||||
sys.stdout.flush()
|
||||
time.sleep(sleepTime)
|
||||
|
||||
print(' ')
|
||||
|
||||
alpha = objectiveParams[0]
|
||||
final_distance = np.linalg.norm(start_position - current_position())
|
||||
finalReturn = final_distance - alpha * total_energy
|
||||
elapsedTime = time.time() - beforeTime
|
||||
print ("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime)
|
||||
return finalReturn
|
||||
@@ -1,6 +1,7 @@
|
||||
import pybullet as p
|
||||
|
||||
from minitaur import Minitaur
|
||||
from minitaur_evaluate import *
|
||||
|
||||
import time
|
||||
import math
|
||||
import numpy as np
|
||||
@@ -10,24 +11,13 @@ def main(unused_args):
|
||||
c = p.connect(p.SHARED_MEMORY)
|
||||
if (c<0):
|
||||
c = p.connect(p.GUI)
|
||||
p.resetSimulation()
|
||||
p.setTimeStep(timeStep)
|
||||
p.loadURDF("plane.urdf")
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
minitaur = Minitaur()
|
||||
amplitude = 0.24795664427
|
||||
speed = 0.2860877729434
|
||||
for i in range(1000):
|
||||
a1 = math.sin(i*speed)*amplitude+1.57
|
||||
a2 = math.sin(i*speed+3.14)*amplitude+1.57
|
||||
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
|
||||
minitaur.applyAction(joint_values)
|
||||
params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539]
|
||||
evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase'
|
||||
energy_weight = 0.01
|
||||
|
||||
p.stepSimulation()
|
||||
# print(minitaur.getBasePosition())
|
||||
time.sleep(timeStep)
|
||||
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
|
||||
print(final_distance)
|
||||
finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep)
|
||||
|
||||
print(finalReturn)
|
||||
|
||||
main(0)
|
||||
|
||||
Reference in New Issue
Block a user