def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320,240)
time.sleep(0.01)
def __init__(self):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False)
p.setRealTimeSimulation(1)
import pybullet as p
import pybullet_data as pd
import math
import time
import numpy as np
from pybullet_envs.examples import panda_sim
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
p.setAdditionalSearchPath(pd.getDataPath())
timeStep = 1. / 60.
p.setTimeStep(timeStep)
p.setGravity(0, -9.8, 0)
panda = panda_sim.PandaSim(p, [0, 0, 0])
while (1):
panda.step()
p.stepSimulation()
time.sleep(timeStep)
# coding: utf-8
import pybullet as p
import pybullet_data
from pybullet_envs.bullet import racecar
import numpy as np
import time
import os
# physicsClient = p.connect(p.DIRECT)
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane100.urdf")
# planeId = p.loadSDF('stadium.sdf')
p.setAdditionalSearchPath(pybullet_data.getDataPath())
r2d2 = p.loadURDF("r2d2.urdf", [2,0,0.5])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# racecar = p.loadURDF('racecar/racecar_differential.urdf', [0,0,0.5])
racecar = racecar.Racecar(p, pybullet_data.getDataPath())
p.setAdditionalSearchPath(os.environ['HOME']+"/atsushi/catkin_ws/src/robotHand_v1/urdf/")
startPos = [0,0,1]
startOrientation = p.getQuaternionFromEuler([0,0,0])
car = p.loadURDF("smahoHand.urdf", startPos, startOrientation)
p.setJointMotorControlArray(car, [2,3], p.VELOCITY_CONTROL, targetVelocities=[6,10], forces=[10,10])
# 2台目
cuid = p.loadURDF("test_car.urdf",[0,1,1], startOrientation)
def startRun(self,
filePath=None,
areaPath=None,
gravity=(0, 0, -9.8),
visual=True,
timeStepDuration=1.0 / 60.):
if self.hasError:
print(
"Error occured during build - can't start with the existing Robot_Module"
)
return False
if filePath is None:
print("Filepath is required")
return False
if len(gravity) != 3:
print("Gravity must be 3 dimension")
return False
self.finishBuild = True
self.WB.prettyPrint(filePath)
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
if visual is True:
cid = p.connect(
p.GUI
) #DIRECT is much faster, but GUI shows the running gait
else:
cid = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
loaded = False
if not areaPath is None:
filename, file_extension = os.path.splitext(str(areaPath))
if file_extension.lower() == ".urdf":
loaded = True
p.loadURDF(str(areaPath))
if file_extension.lower() == ".sdf":
loaded = True
p.loadSDF(str(areaPath))
if loaded == False:
p.loadURDF("plane.urdf")
p.setGravity(gravity[0], gravity[1], gravity[2])
p.setPhysicsEngineParameter(fixedTimeStep=timeStepDuration,
numSolverIterations=5,
numSubSteps=2)
objs = p.loadMJCF(filePath + ".xml",
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.robot = objs[0]
ordered_joints = []
ordered_joint_indices = []
parser = argparse.ArgumentParser()
parser.add_argument('--profile')
self.jdict = {}
for j in range(p.getNumJoints(self.robot)):
info = p.getJointInfo(self.robot, j)
ordered_joint_indices.append(j)
if info[2] != p.JOINT_REVOLUTE: continue
jname = info[1].decode("ascii")
self.jdict[jname] = j
lower, upper = (info[8], info[9])
ordered_joints.append((j, lower, upper))
p.setJointMotorControl2(self.robot,
j,
controlMode=p.VELOCITY_CONTROL,
force=0)
self.frame = 0
self.startTime = time.time()
self.jointSteps = {}
self.startPosition, (qx, qy, qz,
qw) = p.getBasePositionAndOrientation(self.robot)
return True
import pybullet as p
import pybullet_data as pd
import time
import keyboard
import math
p.connect(p.GUI)
pi=3.14159265359
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
orient = p.getQuaternionFromEuler([-pi/2, 0, pi/2])
my_flag = p.URDF_USE_INERTIA_FROM_FILE
p.setAdditionalSearchPath("C:/Users/Joseph\Documents/1_Important/Senior_Design/Simulation/First_App_Test_Stand_urdf7/urdf")
stand = p.loadURDF("First_App_Test_Stand_urdf7.urdf", [0.07, 0, 0], orient, flags=my_flag, useFixedBase=True)
# p.setRealTimeSimulation(1)
p.getNumBodies()
carraigePos = p.addUserDebugParameter("carraige pos",-0.4,0.3,0.25)
x = p.addUserDebugParameter("x pos",-0.02,0.02,0)
y = p.addUserDebugParameter("y pos",-0.035,-0.005,-0.015)
p.setGravity(0,0,-9.81)
upper_off = -0.65+3*math.pi/2
lower_off = -1.5
a1 = 0.02
a2 = 0.02
xc = [0.0099985,0.0099939,0.0099863,0.0099756,0.0099619,0.0099452,0.0099255,0.0099027,0.0098769,0.0098481,0.0098163,0.0097815,0.0097437,0.009703,0.0096593,0.0096126,0.009563,0.0095106,0.0094552,0.0093969,0.0093358,0.0092718,0.009205,0.0091355,0.0090631,0.0089879,0.0089101,0.0088295,0.0087462,0.0086603,0.0085717,0.0084805,0.0083867,0.0082904,0.0081915,0.0080902,0.0079864,0.0078801,0.0077715,0.0076604,0.0075471,0.0074314,0.0073135,0.0071934,0.0070711,0.0069466,0.00682,0.0066913,0.0065606,0.0064279,0.0062932,0.0061566,0.0060182,0.0058779,0.0057358,0.0055919,0.0054464,0.0052992,0.0051504,0.005,0.0048481,0.0046947,0.0045399,0.0043837,0.0042262,0.0040674,0.0039073,0.0037461,0.0035837,0.0034202,0.0032557,0.0030902,0.0029237,0.0027564,0.0025882,0.0024192,0.0022495,0.0020791,0.0019081,0.0017365,0.0015643,0.0013917,0.0012187,0.0010453,0.00087156,0.00069756,0.00052336,0.00034899,0.00017452,0,-0.00017452,-0.00034899,-0.00052336,-0.00069756,-0.00087156,-0.0010453,-0.0012187,-0.0013917,-0.0015643,-0.0017365,-0.0019081,-0.0020791,-0.0022495,-0.0024192,-0.0025882,-0.0027564,-0.0029237,-0.0030902,-0.0032557,-0.0034202,-0.0035837,-0.0037461,-0.0039073,-0.0040674,-0.0042262,-0.0043837,-0.0045399,-0.0046947,-0.0048481,-0.005,-0.0051504,-0.0052992,-0.0054464,-0.0055919,-0.0057358,-0.0058779,-0.0060182,-0.0061566,-0.0062932,-0.0064279,-0.0065606,-0.0066913,-0.00682,-0.0069466,-0.0070711,-0.0071934,-0.0073135,-0.0074314,-0.0075471,-0.0076604,-0.0077715,-0.0078801,-0.0079864,-0.0080902,-0.0081915,-0.0082904,-0.0083867,-0.0084805,-0.0085717,-0.0086603,-0.0087462,-0.0088295,-0.0089101,-0.0089879,-0.0090631,-0.0091355,-0.009205,-0.0092718,-0.0093358,-0.0093969,-0.0094552,-0.0095106,-0.009563,-0.0096126,-0.0096593,-0.009703,-0.0097437,-0.0097815,-0.0098163,-0.0098481,-0.0098769,-0.0099027,-0.0099255,-0.0099452,-0.0099619,-0.0099756,-0.0099863,-0.0099939,-0.0099985,-0.01,-0.0099985,-0.0099939,-0.0099863,-0.0099756,-0.0099619,-0.0099452,-0.0099255,-0.0099027,-0.0098769,-0.0098481,-0.0098163,-0.0097815,-0.0097437,-0.009703,-0.0096593,-0.0096126,-0.009563,-0.0095106,-0.0094552,-0.0093969,-0.0093358,-0.0092718,-0.009205,-0.0091355,-0.0090631,-0.0089879,-0.0089101,-0.0088295,-0.0087462,-0.0086603,-0.0085717,-0.0084805,-0.0083867,-0.0082904,-0.0081915,-0.0080902,-0.0079864,-0.0078801,-0.0077715,-0.0076604,-0.0075471,-0.0074314,-0.0073135,-0.0071934,-0.0070711,-0.0069466,-0.00682,-0.0066913,-0.0065606,-0.0064279,-0.0062932,-0.0061566,-0.0060182,-0.0058779,-0.0057358,-0.0055919,-0.0054464,-0.0052992,-0.0051504,-0.005,-0.0048481,-0.0046947,-0.0045399,-0.0043837,-0.0042262,-0.0040674,-0.0039073,-0.0037461,-0.0035837,-0.0034202,-0.0032557,-0.0030902,-0.0029237,-0.0027564,-0.0025882,-0.0024192,-0.0022495,-0.0020791,-0.0019081,-0.0017365,-0.0015643,-0.0013917,-0.0012187,-0.0010453,-0.00087156,-0.00069756,-0.00052336,-0.00034899,-0.00017452,0,0.00017452,0.00034899,0.00052336,0.00069756,0.00087156,0.0010453,0.0012187,0.0013917,0.0015643,0.0017365,0.0019081,0.0020791,0.0022495,0.0024192,0.0025882,0.0027564,0.0029237,0.0030902,0.0032557,0.0034202,0.0035837,0.0037461,0.0039073,0.0040674,0.0042262,0.0043837,0.0045399,0.0046947,0.0048481,0.005,0.0051504,0.0052992,0.0054464,0.0055919,0.0057358,0.0058779,0.0060182,0.0061566,0.0062932,0.0064279,0.0065606,0.0066913,0.00682,0.0069466,0.0070711,0.0071934,0.0073135,0.0074314,0.0075471,0.0076604,0.0077715,0.0078801,0.0079864,0.0080902,0.0081915,0.0082904,0.0083867,0.0084805,0.0085717,0.0086603,0.0087462,0.0088295,0.0089101,0.0089879,0.0090631,0.0091355,0.009205,0.0092718,0.0093358,0.0093969,0.0094552,0.0095106,0.009563,0.0096126,0.0096593,0.009703,0.0097437,0.0097815,0.0098163,0.0098481,0.0098769,0.0099027,0.0099255,0.0099452,0.0099619,0.0099756,0.0099863,0.0099939,0.0099985,0.01]
yc = [-0.014825,-0.014651,-0.014477,-0.014302,-0.014128,-0.013955,-0.013781,-0.013608,-0.013436,-0.013264,-0.013092,-0.012921,-0.01275,-0.012581,-0.012412,-0.012244,-0.012076,-0.01191,-0.011744,-0.01158,-0.011416,-0.011254,-0.011093,-0.010933,-0.010774,-0.010616,-0.01046,-0.010305,-0.010152,-0.01,-0.0098496,-0.0097008,-0.0095536,-0.0094081,-0.0092642,-0.0091221,-0.0089818,-0.0088434,-0.0087068,-0.0085721,-0.0084394,-0.0083087,-0.00818,-0.0080534,-0.0079289,-0.0078066,-0.0076865,-0.0075686,-0.0074529,-0.0073396,-0.0072285,-0.0071199,-0.0070136,-0.0069098,-0.0068085,-0.0067096,-0.0066133,-0.0065195,-0.0064283,-0.0063397,-0.0062538,-0.0061705,-0.0060899,-0.0060121,-0.0059369,-0.0058645,-0.005795,-0.0057282,-0.0056642,-0.0056031,-0.0055448,-0.0054894,-0.005437,-0.0053874,-0.0053407,-0.005297,-0.0052563,-0.0052185,-0.0051837,-0.0051519,-0.0051231,-0.0050973,-0.0050745,-0.0050548,-0.0050381,-0.0050244,-0.0050137,-0.0050061,-0.0050015,-0.005,-0.0050015,-0.0050061,-0.0050137,-0.0050244,-0.0050381,-0.0050548,-0.0050745,-0.0050973,-0.0051231,-0.0051519,-0.0051837,-0.0052185,-0.0052563,-0.005297,-0.0053407,-0.0053874,-0.005437,-0.0054894,-0.0055448,-0.0056031,-0.0056642,-0.0057282,-0.005795,-0.0058645,-0.0059369,-0.0060121,-0.0060899,-0.0061705,-0.0062538,-0.0063397,-0.0064283,-0.0065195,-0.0066133,-0.0067096,-0.0068085,-0.0069098,-0.0070136,-0.0071199,-0.0072285,-0.0073396,-0.0074529,-0.0075686,-0.0076865,-0.0078066,-0.0079289,-0.0080534,-0.00818,-0.0083087,-0.0084394,-0.0085721,-0.0087068,-0.0088434,-0.0089818,-0.0091221,-0.0092642,-0.0094081,-0.0095536,-0.0097008,-0.0098496,-0.01,-0.010152,-0.010305,-0.01046,-0.010616,-0.010774,-0.010933,-0.011093,-0.011254,-0.011416,-0.01158,-0.011744,-0.01191,-0.012076,-0.012244,-0.012412,-0.012581,-0.01275,-0.012921,-0.013092,-0.013264,-0.013436,-0.013608,-0.013781,-0.013955,-0.014128,-0.014302,-0.014477,-0.014651,-0.014825,-0.015,-0.015175,-0.015349,-0.015523,-0.015698,-0.015872,-0.016045,-0.016219,-0.016392,-0.016564,-0.016736,-0.016908,-0.017079,-0.01725,-0.017419,-0.017588,-0.017756,-0.017924,-0.01809,-0.018256,-0.01842,-0.018584,-0.018746,-0.018907,-0.019067,-0.019226,-0.019384,-0.01954,-0.019695,-0.019848,-0.02,-0.02015,-0.020299,-0.020446,-0.020592,-0.020736,-0.020878,-0.021018,-0.021157,-0.021293,-0.021428,-0.021561,-0.021691,-0.02182,-0.021947,-0.022071,-0.022193,-0.022314,-0.022431,-0.022547,-0.02266,-0.022771,-0.02288,-0.022986,-0.02309,-0.023192,-0.02329,-0.023387,-0.02348,-0.023572,-0.02366,-0.023746,-0.023829,-0.02391,-0.023988,-0.024063,-0.024135,-0.024205,-0.024272,-0.024336,-0.024397,-0.024455,-0.024511,-0.024563,-0.024613,-0.024659,-0.024703,-0.024744,-0.024781,-0.024816,-0.024848,-0.024877,-0.024903,-0.024925,-0.024945,-0.024962,-0.024976,-0.024986,-0.024994,-0.024998,-0.025,-0.024998,-0.024994,-0.024986,-0.024976,-0.024962,-0.024945,-0.024925,-0.024903,-0.024877,-0.024848,-0.024816,-0.024781,-0.024744,-0.024703,-0.024659,-0.024613,-0.024563,-0.024511,-0.024455,-0.024397,-0.024336,-0.024272,-0.024205,-0.024135,-0.024063,-0.023988,-0.02391,-0.023829,-0.023746,-0.02366,-0.023572,-0.02348,-0.023387,-0.02329,-0.023192,-0.02309,-0.022986,-0.02288,-0.022771,-0.02266,-0.022547,-0.022431,-0.022314,-0.022193,-0.022071,-0.021947,-0.02182,-0.021691,-0.021561,-0.021428,-0.021293,-0.021157,-0.021018,-0.020878,-0.020736,-0.020592,-0.020446,-0.020299,-0.02015,-0.02,-0.019848,-0.019695,-0.01954,-0.019384,-0.019226,-0.019067,-0.018907,-0.018746,-0.018584,-0.01842,-0.018256,-0.01809,-0.017924,-0.017756,-0.017588,-0.017419,-0.01725,-0.017079,-0.016908,-0.016736,-0.016564,-0.016392,-0.016219,-0.016045,-0.015872,-0.015698,-0.015523,-0.015349,-0.015175,-0.015]
xc2 = [0.0049992,0.004997,0.0049931,0.0049878,0.004981,0.0049726,0.0049627,0.0049513,0.0049384,0.004924,0.0049081,0.0048907,0.0048719,0.0048515,0.0048296,0.0048063,0.0047815,0.0047553,0.0047276,0.0046985,0.0046679,0.0046359,0.0046025,0.0045677,0.0045315,0.004494,0.004455,0.0044147,0.0043731,0.0043301,0.0042858,0.0042402,0.0041934,0.0041452,0.0040958,0.0040451,0.0039932,0.0039401,0.0038857,0.0038302,0.0037735,0.0037157,0.0036568,0.0035967,0.0035355,0.0034733,0.00341,0.0033457,0.0032803,0.0032139,0.0031466,0.0030783,0.0030091,0.0029389,0.0028679,0.002796,0.0027232,0.0026496,0.0025752,0.0025,0.002424,0.0023474,0.00227,0.0021919,0.0021131,0.0020337,0.0019537,0.001873,0.0017918,0.0017101,0.0016278,0.0015451,0.0014619,0.0013782,0.0012941,0.0012096,0.0011248,0.0010396,0.00095404,0.00086824,0.00078217,0.00069587,0.00060935,0.00052264,0.00043578,0.00034878,0.00026168,0.0001745,8.7262e-05,0,-8.7262e-05,-0.0001745,-0.00026168,-0.00034878,-0.00043578,-0.00052264,-0.00060935,-0.00069587,-0.00078217,-0.00086824,-0.00095404,-0.0010396,-0.0011248,-0.0012096,-0.0012941,-0.0013782,-0.0014619,-0.0015451,-0.0016278,-0.0017101,-0.0017918,-0.001873,-0.0019537,-0.0020337,-0.0021131,-0.0021919,-0.00227,-0.0023474,-0.002424,-0.0025,-0.0025752,-0.0026496,-0.0027232,-0.002796,-0.0028679,-0.0029389,-0.0030091,-0.0030783,-0.0031466,-0.0032139,-0.0032803,-0.0033457,-0.00341,-0.0034733,-0.0035355,-0.0035967,-0.0036568,-0.0037157,-0.0037735,-0.0038302,-0.0038857,-0.0039401,-0.0039932,-0.0040451,-0.0040958,-0.0041452,-0.0041934,-0.0042402,-0.0042858,-0.0043301,-0.0043731,-0.0044147,-0.004455,-0.004494,-0.0045315,-0.0045677,-0.0046025,-0.0046359,-0.0046679,-0.0046985,-0.0047276,-0.0047553,-0.0047815,-0.0048063,-0.0048296,-0.0048515,-0.0048719,-0.0048907,-0.0049081,-0.004924,-0.0049384,-0.0049513,-0.0049627,-0.0049726,-0.004981,-0.0049878,-0.0049931,-0.004997,-0.0049992,-0.005,-0.0049992,-0.004997,-0.0049931,-0.0049878,-0.004981,-0.0049726,-0.0049627,-0.0049513,-0.0049384,-0.004924,-0.0049081,-0.0048907,-0.0048719,-0.0048515,-0.0048296,-0.0048063,-0.0047815,-0.0047553,-0.0047276,-0.0046985,-0.0046679,-0.0046359,-0.0046025,-0.0045677,-0.0045315,-0.004494,-0.004455,-0.0044147,-0.0043731,-0.0043301,-0.0042858,-0.0042402,-0.0041934,-0.0041452,-0.0040958,-0.0040451,-0.0039932,-0.0039401,-0.0038857,-0.0038302,-0.0037735,-0.0037157,-0.0036568,-0.0035967,-0.0035355,-0.0034733,-0.00341,-0.0033457,-0.0032803,-0.0032139,-0.0031466,-0.0030783,-0.0030091,-0.0029389,-0.0028679,-0.002796,-0.0027232,-0.0026496,-0.0025752,-0.0025,-0.002424,-0.0023474,-0.00227,-0.0021919,-0.0021131,-0.0020337,-0.0019537,-0.001873,-0.0017918,-0.0017101,-0.0016278,-0.0015451,-0.0014619,-0.0013782,-0.0012941,-0.0012096,-0.0011248,-0.0010396,-0.00095404,-0.00086824,-0.00078217,-0.00069587,-0.00060935,-0.00052264,-0.00043578,-0.00034878,-0.00026168,-0.0001745,-8.7262e-05,0,8.7262e-05,0.0001745,0.00026168,0.00034878,0.00043578,0.00052264,0.00060935,0.00069587,0.00078217,0.00086824,0.00095404,0.0010396,0.0011248,0.0012096,0.0012941,0.0013782,0.0014619,0.0015451,0.0016278,0.0017101,0.0017918,0.001873,0.0019537,0.0020337,0.0021131,0.0021919,0.00227,0.0023474,0.002424,0.0025,0.0025752,0.0026496,0.0027232,0.002796,0.0028679,0.0029389,0.0030091,0.0030783,0.0031466,0.0032139,0.0032803,0.0033457,0.00341,0.0034733,0.0035355,0.0035967,0.0036568,0.0037157,0.0037735,0.0038302,0.0038857,0.0039401,0.0039932,0.0040451,0.0040958,0.0041452,0.0041934,0.0042402,0.0042858,0.0043301,0.0043731,0.0044147,0.004455,0.004494,0.0045315,0.0045677,0.0046025,0.0046359,0.0046679,0.0046985,0.0047276,0.0047553,0.0047815,0.0048063,0.0048296,0.0048515,0.0048719,0.0048907,0.0049081,0.004924,0.0049384,0.0049513,0.0049627,0.0049726,0.004981,0.0049878,0.0049931,0.004997,0.0049992,0.005]
yc2 = [-0.014913,-0.014826,-0.014738,-0.014651,-0.014564,-0.014477,-0.014391,-0.014304,-0.014218,-0.014132,-0.014046,-0.01396,-0.013875,-0.01379,-0.013706,-0.013622,-0.013538,-0.013455,-0.013372,-0.01329,-0.013208,-0.013127,-0.013046,-0.012966,-0.012887,-0.012808,-0.01273,-0.012653,-0.012576,-0.0125,-0.012425,-0.01235,-0.012277,-0.012204,-0.012132,-0.012061,-0.011991,-0.011922,-0.011853,-0.011786,-0.01172,-0.011654,-0.01159,-0.011527,-0.011464,-0.011403,-0.011343,-0.011284,-0.011226,-0.01117,-0.011114,-0.01106,-0.011007,-0.010955,-0.010904,-0.010855,-0.010807,-0.01076,-0.010714,-0.01067,-0.010627,-0.010585,-0.010545,-0.010506,-0.010468,-0.010432,-0.010397,-0.010364,-0.010332,-0.010302,-0.010272,-0.010245,-0.010218,-0.010194,-0.01017,-0.010149,-0.010128,-0.010109,-0.010092,-0.010076,-0.010062,-0.010049,-0.010037,-0.010027,-0.010019,-0.010012,-0.010007,-0.010003,-0.010001,-0.01,-0.010001,-0.010003,-0.010007,-0.010012,-0.010019,-0.010027,-0.010037,-0.010049,-0.010062,-0.010076,-0.010092,-0.010109,-0.010128,-0.010149,-0.01017,-0.010194,-0.010218,-0.010245,-0.010272,-0.010302,-0.010332,-0.010364,-0.010397,-0.010432,-0.010468,-0.010506,-0.010545,-0.010585,-0.010627,-0.01067,-0.010714,-0.01076,-0.010807,-0.010855,-0.010904,-0.010955,-0.011007,-0.01106,-0.011114,-0.01117,-0.011226,-0.011284,-0.011343,-0.011403,-0.011464,-0.011527,-0.01159,-0.011654,-0.01172,-0.011786,-0.011853,-0.011922,-0.011991,-0.012061,-0.012132,-0.012204,-0.012277,-0.01235,-0.012425,-0.0125,-0.012576,-0.012653,-0.01273,-0.012808,-0.012887,-0.012966,-0.013046,-0.013127,-0.013208,-0.01329,-0.013372,-0.013455,-0.013538,-0.013622,-0.013706,-0.01379,-0.013875,-0.01396,-0.014046,-0.014132,-0.014218,-0.014304,-0.014391,-0.014477,-0.014564,-0.014651,-0.014738,-0.014826,-0.014913,-0.015,-0.015087,-0.015174,-0.015262,-0.015349,-0.015436,-0.015523,-0.015609,-0.015696,-0.015782,-0.015868,-0.015954,-0.01604,-0.016125,-0.01621,-0.016294,-0.016378,-0.016462,-0.016545,-0.016628,-0.01671,-0.016792,-0.016873,-0.016954,-0.017034,-0.017113,-0.017192,-0.01727,-0.017347,-0.017424,-0.0175,-0.017575,-0.01765,-0.017723,-0.017796,-0.017868,-0.017939,-0.018009,-0.018078,-0.018147,-0.018214,-0.01828,-0.018346,-0.01841,-0.018473,-0.018536,-0.018597,-0.018657,-0.018716,-0.018774,-0.01883,-0.018886,-0.01894,-0.018993,-0.019045,-0.019096,-0.019145,-0.019193,-0.01924,-0.019286,-0.01933,-0.019373,-0.019415,-0.019455,-0.019494,-0.019532,-0.019568,-0.019603,-0.019636,-0.019668,-0.019698,-0.019728,-0.019755,-0.019782,-0.019806,-0.01983,-0.019851,-0.019872,-0.019891,-0.019908,-0.019924,-0.019938,-0.019951,-0.019963,-0.019973,-0.019981,-0.019988,-0.019993,-0.019997,-0.019999,-0.02,-0.019999,-0.019997,-0.019993,-0.019988,-0.019981,-0.019973,-0.019963,-0.019951,-0.019938,-0.019924,-0.019908,-0.019891,-0.019872,-0.019851,-0.01983,-0.019806,-0.019782,-0.019755,-0.019728,-0.019698,-0.019668,-0.019636,-0.019603,-0.019568,-0.019532,-0.019494,-0.019455,-0.019415,-0.019373,-0.01933,-0.019286,-0.01924,-0.019193,-0.019145,-0.019096,-0.019045,-0.018993,-0.01894,-0.018886,-0.01883,-0.018774,-0.018716,-0.018657,-0.018597,-0.018536,-0.018473,-0.01841,-0.018346,-0.01828,-0.018214,-0.018147,-0.018078,-0.018009,-0.017939,-0.017868,-0.017796,-0.017723,-0.01765,-0.017575,-0.0175,-0.017424,-0.017347,-0.01727,-0.017192,-0.017113,-0.017034,-0.016954,-0.016873,-0.016792,-0.01671,-0.016628,-0.016545,-0.016462,-0.016378,-0.016294,-0.01621,-0.016125,-0.01604,-0.015954,-0.015868,-0.015782,-0.015696,-0.015609,-0.015523,-0.015436,-0.015349,-0.015262,-0.015174,-0.015087,-0.015]
idx = 77;
walk_x = [0.0122,-0.0116,-0.011,-0.0104,-0.0098,-0.0092,-0.0086,-0.008,-0.0074,-0.0068,-0.0062,-0.0056,-0.005,-0.0044,-0.0038,-0.0032,-0.0026,-0.002,-0.0014,-0.0008,-0.0002,0.0004,0.001,0.0016,0.0022,0.0028,0.0034,0.004,0.0046,0.0052,0.0058,0.0064,0.007,0.0076,0.0082,0.0088,0.0094,0.01,0.0106,0.0112,0.0118,0.0122,0.0116,0.011,0.0104,0.0098,0.0092,0.0086,0.008,0.0074,0.0068,0.0062,0.0056,0.005,0.0044,0.0038,0.0032,0.0026,0.002,0.0014,0.0008,0.0002,-0.0004,-0.001,-0.0016,-0.0022,-0.0028,-0.0034,-0.004,-0.0046,-0.0052,-0.0058,-0.0064,-0.007,-0.0076,-0.0082,-0.0088,-0.0094,-0.01,-0.0106,-0.0112,-0.0118,-0.0124]
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model and
defines the cameras of the model.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
Returns:
boolean - True if the method ran correctly, False otherwise
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.36 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 0.36]
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict["Head"].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict[side + "Hip"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Bicep"].getIndex(),
self.link_dict[side + "ForeArm"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Pelvis"].getIndex(),
self.link_dict[side + "Thigh"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Tibia"].getIndex(),
self.link_dict[side.lower() + "_ankle"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger11_link"].getIndex(),
self.link_dict[side + "Finger13_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger21_link"].getIndex(),
self.link_dict[side + "Finger23_link"].getIndex(),
0,
physicsClientId=self.physics_client)
for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[base_link].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint in self.joint_dict.values():
pybullet.resetJointState(self.robot_model, joint.getIndex(), 0.0)
pybullet.removeConstraint(balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
self.camera_top = CameraRgb(self.robot_model,
self.link_dict["CameraTop_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
self.camera_bottom = CameraRgb(
self.robot_model,
self.link_dict["CameraBottom_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
def _setup_scene(self):
self._simulation_manager = SimulationManager()
self._client = self._simulation_manager.launchSimulation(
gui=self._gui, auto_step=False, use_shared_memory=True)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self._robot = self._simulation_manager.spawnPepper(
self._client, spawn_ground_plane=False)
self._robot.goToPosture("Stand", 1.0)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._robot.setAngles(["KneePitch", "HipPitch", "LShoulderPitch"],
[0.33, -0.9, -0.6], [0.5] * 3)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._table_init_pos = [0.35, 0, 0]
self._table_init_ori = [0, 0, 0, 1]
self._obj_init_pos = [0.35, 0, 0.65]
self._obj_init_ori = [0, 0, 0, 1]
dirname = os.path.dirname(__file__)
assets_path = os.path.join(dirname, '../assets/models')
p.setAdditionalSearchPath(assets_path, physicsClientId=self._client)
self._floor = p.loadURDF("floor/floor.urdf",
physicsClientId=self._client,
useFixedBase=True)
self._table = p.loadURDF(
"adjustable_table/adjustable_table.urdf",
self._table_init_pos,
self._table_init_ori,
physicsClientId=self._client,
)
self._obj = p.loadURDF(
"brick/brick.urdf",
self._obj_init_pos,
self._obj_init_ori,
physicsClientId=self._client,
flags=p.URDF_USE_INERTIA_FROM_FILE,
)
# Let things fall down
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self.joints_initial_pose = self._robot.getAnglesPosition(
self._robot.joint_dict.keys())
self._obj_start_pos = p.getBasePositionAndOrientation(
self._obj, physicsClientId=self._client)[0]
if self._gui:
# load ghosts
self._ghost = p.loadURDF(
"brick/brick_ghost.urdf",
self._obj_start_pos,
self._obj_init_ori,
physicsClientId=self._client,
useFixedBase=True,
)
# Setup camera
if self._use_top_camera:
self._cam_top = self._robot.subscribeCamera(
PepperVirtual.ID_CAMERA_TOP)
self._cam_bottom = self._robot.subscribeCamera(
PepperVirtual.ID_CAMERA_BOTTOM)
if self._use_depth_camera:
self._cam_depth = self._robot.subscribeCamera(
PepperVirtual.ID_CAMERA_DEPTH)
# prepare everything for the physics client
physicsClient = p.connect(
p.GUI) # p.GUI for graphical, or p.DIRECT for non-graphical version
n_steps = 1500000
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(
0
) # only if this is set to 0 and the simulation is done with explicit steps will the torque control work correctly
p.setPhysicsEngineParameter(
enableFileCaching=0) # xx todo: check if this makes things faster
time_step = 0.01
p.setTimeStep(time_step)
# load the ground plane into pybullet simulator
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # defines the path used by p.loadURDF
planeId = p.loadURDF("plane.urdf")
# load the test urdf into pybullet
bodyUniqueId = p.loadURDF(test_urdf,
physicsClientId=physicsClient,
useFixedBase=1)
# run a few simulation steps
for i in range(n_steps):
# todo: change structure such that spring torque is always applied and joints are always turned off automatically.
# todo: add assertion in apply_actuatorTorque that the actuator_nr is not larger than the number of actuators, and same for axis_nr
# xx todo: passive_torque should always be applied in every timestep. how can this be done quickly?
time.sleep(1000)
p.stepSimulation()
import walkGenerator
# motor parameters
motor_kp = 0.5
motor_kd = 0.5
motor_torque = 1.5
motor_max_velocity = 5.0
# physics parameters
fixedTimeStep = 1. / 2000
numSolverIterations = 200
physicsClient = p.connect(p.GUI)
p.setTimeStep(fixedTimeStep)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # to load plane.urdf
p.setGravity(0, 0, 0)
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=10,
cameraPitch=-0,
cameraTargetPosition=[0.4, 0, 0.1])
planeId = p.loadURDF("plane.urdf")
humanoid = p.loadURDF(
r"C:\Users\Scientist\ml_robotics\bipedal_robot_simulation\humanoid_leg_12dof.7.urdf",
[0, 0, 0.1],
p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=False)
import numpy as np
import pybullet as p
import pybullet_data
import sys, os, time
script_path = os.path.dirname(os.path.realpath(__file__))
os.sys.path.append(os.path.realpath(script_path + '/../'))
from src.buff_digit import Buff_digit
if __name__ == '__main__':
client = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setRealTimeSimulation(1)
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setAdditionalSearchPath('../models')
floor = p.loadURDF('floor/floor.urdf', useFixedBase=True)
robot = Buff_digit(client)
time.sleep(25)
table = p.loadURDF('table/table.urdf', [1.0, 0, 0],
p.getQuaternionFromEuler([0, 0, 1.57]),
useFixedBase=True)
coke = p.loadURDF('coke/coke.urdf', [0.7, -0.3, 0.9])
pick_pose = [0.2, -0.25, 0]
base_limits = ((-2.5, -2.5), (2.5, 2.5))
robot.plan_and_drive_to_pose(pick_pose,
base_limits,
obstacles=[table, coke])
# video requires ffmpeg available in path
createVideo = False
fps = 240.
timeStep = 1. / fps
if createVideo:
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=0 --mp4=\"pybullet_grasp.mp4\" --mp4fps=" + str(fps))
else:
p = bc.BulletClient(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000)
p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0])
p.setAdditionalSearchPath('/home/mihai/PycharmProjects/HER-lightning/Bullet-HRL/assets/')
p.setTimeStep(timeStep)
p.setGravity(0,-9.8,0)
np_random, seed = seeding.np_random(1)
panda = panda_sim.FetchBulletSim(p,[0,0,0], np_random=np_random)
panda.control_dt = timeStep
# logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
# panda.bullet_client.submitProfileTiming("start")
for i in range(100000):
# panda.bullet_client.submitProfileTiming("full_step")
panda.step(np.array([0.0, -0.2, 0.0, 0.01]), rendering=True, time_step=timeStep)
# panda.bullet_client.stepSimulation()
def _load_plane(self):
# Load a URDF plane
pb.setAdditionalSearchPath(pbd.getDataPath())
self.plane_id = pb.loadURDF('plane.urdf') # type: int
import pybullet as p
import time
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
visualShift = [0,0,0]
collisionShift = [0,0,0]
inertiaShift = [0,0,-0.5]
meshScale=[1,1,1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale)
p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False)
while (1):
p.stepSimulation()
time.sleep(1./240.)
import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
p.setAdditionalSearchPath("./urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("one_leg.urdf",cubeStartPos, cubeStartOrientation)
#for i in range (10000):
# p.stepSimulation()
# time.sleep(1./240.)
#cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
#print(cubePos,cubeOrn)
p.setRealTimeSimulation(1)
for i in range (10000):
p.stepSimulation()
time.sleep(1./240.)
p.disconnect()
def __init__(self, render=False, time_step=1 / 200., frame_skip=1):
self._time_step = time_step
self._frame_skip = frame_skip
self._render = render
if render:
bullet_client.connect(bullet_client.GUI)
bullet_client.configureDebugVisualizer(
bullet_client.COV_ENABLE_GUI, 0)
else:
bullet_client.connect(bullet_client.DIRECT)
bullet_client.setAdditionalSearchPath(pd.getDataPath())
bullet_client.setTimeStep(time_step)
bullet_client.setGravity(0., 0., -9.81)
flags = bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES
bullet_client.loadURDF("plane.urdf",
np.array([0., 0., 0.]),
flags=flags,
useFixedBase=True)
self.robot_id = bullet_client.loadURDF(dir_path +
'/urdf/panda_chef.urdf',
np.array([0, 0, 0]),
useFixedBase=True,
flags=flags)
self.pancake_id = bullet_client.loadURDF(
dir_path + '/urdf/sphere_with_restitution.urdf',
init_pizza_pose,
init_pancake_rot,
flags=flags)
# setting up the bounds for the action space
self._set_cmd = deepcopy(zero_ee_pose)
self._bnds = (np.array([0.1] * 3), np.array([0.4] * 3))
self.low_bnds = (self._set_cmd[0] - self._bnds[0],
self._set_cmd[1] - self._bnds[1])
self.high_bnds = (self._set_cmd[0] + self._bnds[0],
self._set_cmd[1] + self._bnds[1])
self.action_scale = (np.array([0.05, 0.,
0.05]), np.array([0., 0.7, 0.]))
self.action_space = Box(low=-np.ones(6), high=np.ones(6))
# bullet_client.addUserDebugLine([0.4, 0., 0.1], [0.9, 0., 0.1])
# bullet_client.addUserDebugLine([0.9, 0., 0.1], [0.9, 0., 0.7])
# bullet_client.addUserDebugLine([0.9, 0., 0.7], [0.4, 0., 0.7])
# bullet_client.addUserDebugLine([0.4, 0., 0.7], [0.4, 0., 0.1])
# set up the coordinate sys for the pizza
# bullet_client.addUserDebugText("baseLink", [0,0,0.05],
# textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=self.pancake_id)
# created some coordinate systems for debugging
draw_coordinate(self.pancake_id)
draw_coordinate(self.robot_id)
draw_coordinate(self.robot_id, parentLinkIndex=pandaEndEffectorIndex)
# TODO: create coordinate for the target where you want the pancake to end up
self.reset()
rew, obs = self.get_rew_obs()
self.observation_space = Box(low=-np.inf, high=np.inf, shape=obs.shape)
def reset(self):
self.terminated = 0
self._repositioning = False
self._env_step = 0
self.penalty = 0
#physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(self._urdfRoot)
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -0.3])
#p.loadURDF("plane.urdf", [0, 0, -0.3], useFixedBase=True)
p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 1.000000,
0.00000, -0.050000, 0.000000, 0.000000, 0.707106781187,
.707106781187)
self._goalx = 0.8
self._goaly = 0
self._goalang = 3.1415925438
self._goalorn = p.getQuaternionFromEuler([0, 0, self._goalang])
xpos = 0.8 + random.uniform(-0.1, 0.1)
#ypos = 0 - 0.6 * random.random()
ypos = 0 + random.uniform(-0.1, 0.1)
ang = 3.1415925438 * random.random()
orn = p.getQuaternionFromEuler([0, 0, ang])
"""
self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), xpos, ypos, 0.58,
orn[0], orn[1], orn[2], orn[3])
"""
p.setGravity(0, 0, -10)
self._robonyan = robonyan.Robonyan(urdfRootPath=self._urdfRoot,
timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
urdf_pattern = os.path.join(self._urdfRoot,
'random_urdfs/00[0-9]/*.urdf')
found_object_directories = glob.glob(urdf_pattern)
total_num_objects = len(found_object_directories)
selected_object = np.random.choice(np.arange(total_num_objects))
selected_object_filename = found_object_directories[selected_object]
self.blockUid = p.loadURDF((selected_object_filename), xpos, ypos,
0.58, orn[0], orn[1], orn[2], orn[3])
for _ in range(500):
p.stepSimulation()
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
self.blockPos, self.blockOrn = blockPos, blockOrn
self.ViewPos = [self._goalx, self._goaly, self.blockPos[2]]
#blockPos = np.array(blockPos)
R_position = [
self._goalx, self._goaly, blockPos[2] + 0.2, 0, 0.785398, 0
]
L_position = [
blockPos[0], blockPos[1] + 0.6, blockPos[2] + 0.1, 0, 0, -1.5708
]
self._robonyan.resetHandPosition(R_position, L_position)
p.stepSimulation()
self._observation = self.getExtendedObservation()
self._force = [
self._observation[1][3], self._observation[1][4],
self._observation[1][5]
]
return np.array(self._observation)
def __init__(self, stepsize=1e-3, realtime=0):
self.t = 0.0
self.stepsize = stepsize
self.realtime = realtime
self.control_mode = "torque"
self.position_control_gain_p = [
0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01
]
self.position_control_gain_d = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.max_torque = [100, 100, 100, 100, 100, 100, 100]
# connect pybullet
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=30,
cameraPitch=-20,
cameraTargetPosition=[0, 0, 0.5])
p.resetSimulation()
p.setTimeStep(self.stepsize)
p.setRealTimeSimulation(self.realtime)
p.setGravity(0, 0, -9.81)
# load models
p.setAdditionalSearchPath("../models")
self.plane = p.loadURDF("plane/plane.urdf", useFixedBase=True)
p.changeDynamics(self.plane, -1, restitution=.95)
# ball
self.ball = p.loadURDF("ball/ball.urdf")
p.changeDynamics(self.ball,
-1,
restitution=.95,
linearDamping=1e-2,
angularDamping=1e-2)
self.default_ball_pos = [0.5, 0, 0.1]
self.default_ball_ori = [0, 0, 0, 1]
p.resetBasePositionAndOrientation(self.ball, self.default_ball_pos,
self.default_ball_ori)
# robot
self.robot = p.loadURDF("panda/panda.urdf",
useFixedBase=True,
flags=p.URDF_USE_SELF_COLLISION)
# robot parameters
self.dof = p.getNumJoints(
self.robot
) - 1 # Virtual fixed joint between the flange and last link
print(self.dof)
if self.dof != 7:
raise Exception('wrong urdf file: number of joints is not 7')
self.joints = []
self.q_min = []
self.q_max = []
self.target_pos = []
self.target_torque = []
for j in range(self.dof):
joint_info = p.getJointInfo(self.robot, j)
self.joints.append(j)
self.q_min.append(joint_info[8])
self.q_max.append(joint_info[9])
self.target_pos.append((self.q_min[j] + self.q_max[j]) / 2.0)
self.target_torque.append(0.)
self.reset()
def setup(self, env_config, training_config, gui):
self.action_interval = env_config["action_interval"]
self.episode_length = env_config["episode_length"]
self.simulation_steps_per_action_step = int(
self.action_interval / BaseEnv.SIMULATION_TIMESTEP)
self.episode_counts = 0
self.action_type = 'delta'\
if 'action_type' not in env_config \
else env_config['action_type']
self.observations = None
self.gui = gui
self.ray_id = None
# Get variables from config
self.max_ur5s_count = env_config['max_ur5s_count']
self.max_task_ur5s_count = env_config['max_ur5s_count']
self.min_task_ur5s_count = env_config['min_ur5s_count']
self.survival_penalty = env_config['reward']['survival_penalty']
self.workspace_radius = env_config['workspace_radius']
self.individually_reach_target = \
env_config['reward']['individually_reach_target']
self.collectively_reach_target = \
env_config['reward']['collectively_reach_target']
self.cooperative_individual_reach_target = \
env_config['reward']['cooperative_individual_reach_target']
self.collision_penalty = env_config['reward']['collision_penalty']
proximity_config = env_config['reward']['proximity_penalty']
self.proximity_penalty_distance = proximity_config['max_distance']
self.proximity_penalty = proximity_config['penalty']
self.delta_reward = env_config['reward']['delta']
self.terminate_on_collectively_reach_target = env_config[
'terminate_on_collectively_reach_target']
self.terminate_on_collision = env_config['terminate_on_collision']
self.position_tolerance = env_config['position_tolerance']
self.orientation_tolerance = env_config['orientation_tolerance']
self.stop_ur5_after_reach = False\
if 'stop_ur5_after_reach' not in env_config \
else env_config['stop_ur5_after_reach']
self.finish_task_in_episode = False
self.centralized_policy = False \
if 'centralized_policy' not in training_config\
else training_config['centralized_policy']
self.centralized_critic = False \
if 'centralized_critic' not in training_config\
else training_config['centralized_critic']
self.curriculum = env_config['curriculum']
self.retry_on_fail = env_config['retry_on_fail']
self.failed_in_task = False
self.task_manager = None
self.episode_policy_instance_keys = None
self.memory_cluster_map = {}
self.collision_distance = \
env_config['collision_distance']
self.curriculum_level = -1
self.min_task_difficulty = 0.
self.max_task_difficulty = 100.0
if self.logger is not None:
self.set_level(ray.get(self.logger.get_curriculum_level.remote()))
self.expert_trajectory_threshold = 0.4\
if 'expert_trajectory_threshold' not in env_config\
else env_config['expert_trajectory_threshold']
if self.gui:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf", [0, 0, -self.collision_distance - 0.01])
p.setRealTimeSimulation(0)
p.setGravity(0, 0, -9.81)
self.all_ur5s = [] # reference to all UR5's in scene
self.ur5_episode_memories = None
# Keep track of episode progress
self.current_step = 0
self.terminate_episode = False
# Visualization
if self.gui:
self.real_time_debug = p.addUserDebugParameter(
'real-time', 0.0, 1.0, 1.0)
self.viewer = None
self.on_setup(env_config, training_config)
self.setup_action_observation(training_config['observations'])
self.setup_ur5s(env_config)
def __init__(self,
assets_root,
task=None,
disp=False,
shared_memory=False,
hz=240):
"""Creates OpenAI Gym-style environment with PyBullet.
Args:
assets_root: root directory of assets.
task: the task to use. If None, the user must call set_task for the
environment to work properly.
disp: show environment with PyBullet's built-in display viewer.
shared_memory: run with shared memory.
hz: PyBullet physics simulation step speed. Set to 480 for deformables.
Raises:
RuntimeError: if pybullet cannot load fileIOPlugin.
"""
self.pix_size = 0.003125
self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []}
self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi
self.agent_cams = cameras.RealSenseD415.CONFIG
self.assets_root = assets_root
color_tuple = [
gym.spaces.Box(0,
255,
config['image_size'] + (3, ),
dtype=np.uint8) for config in self.agent_cams
]
depth_tuple = [
gym.spaces.Box(0.0, 20.0, config['image_size'], dtype=np.float32)
for config in self.agent_cams
]
self.observation_space = gym.spaces.Dict({
'color':
gym.spaces.Tuple(color_tuple),
'depth':
gym.spaces.Tuple(depth_tuple),
})
self.position_bounds = gym.spaces.Box(low=np.array([0.25, -0.5, 0.],
dtype=np.float32),
high=np.array([0.75, 0.5, 0.28],
dtype=np.float32),
shape=(3, ),
dtype=np.float32)
self.action_space = gym.spaces.Dict({
'pose0':
gym.spaces.Tuple((self.position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32))),
'pose1':
gym.spaces.Tuple((self.position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32)))
})
# Start PyBullet.
disp_option = p.DIRECT
if disp:
disp_option = p.GUI
if shared_memory:
disp_option = p.SHARED_MEMORY
client = p.connect(disp_option)
file_io = p.loadPlugin('fileIOPlugin', physicsClientId=client)
if file_io < 0:
raise RuntimeError('pybullet: cannot load FileIO!')
if file_io >= 0:
p.executePluginCommand(file_io,
textArgument=assets_root,
intArgs=[p.AddFileIOAction],
physicsClientId=client)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setAdditionalSearchPath(assets_root)
p.setAdditionalSearchPath(tempfile.gettempdir())
p.setTimeStep(1. / hz)
# If using --disp, move default camera closer to the scene.
if disp:
target = p.getDebugVisualizerCamera()[11]
p.resetDebugVisualizerCamera(cameraDistance=1.1,
cameraYaw=90,
cameraPitch=-25,
cameraTargetPosition=target)
if task:
self.set_task(task)
for k in data.keys():
fn_k = "{}_{}.h5".format(id_str, k)
outputFn = os.path.join(outputDir, fn_k)
dd.io.save(outputFn, newData[k])
self.dataList = []
self.id += 1
log = Log("data/grasp")
# Initialize World
logging.info("Initializing world")
physicsClient = pb.connect(pb.DIRECT)
pb.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
pb.setGravity(0, 0, -9.81) # Major Tom to planet Earth
# Initialize digits
digits = tacto.Sensor(width=240, height=320, visualize_gui=False)
pb.resetDebugVisualizerCamera(
cameraDistance=0.6,
cameraYaw=15,
cameraPitch=-20,
# cameraTargetPosition=[-1.20, 0.69, -0.77],
cameraTargetPosition=[0.5, 0, 0.08],
)
planeId = pb.loadURDF("plane.urdf") # Create plane
def __init__(self, config):
self.config = config
self.seed = self.config["seed"]
if self.seed is not None:
self.set_seed(self.seed)
else:
rnd_seed = int((time.time() % 1) * 10000000)
self.set_seed(rnd_seed)
if (self.config["animate"]):
self.client_ID = p.connect(p.GUI)
print(" --Starting GUI mode-- ")
else:
self.client_ID = p.connect(p.DIRECT)
assert self.client_ID != -1, "Physics client failed to connect"
# Environment parameters
self.obs_dim = 18 * 3 + 12
self.act_dim = 18
self.observation_space = spaces.Box(low=-1, high=1, shape=(self.obs_dim,), dtype=np.float32)
self.action_space = spaces.Box(low=-1, high=1, shape=(self.act_dim,), dtype=np.float32)
self.joints_rads_low = np.array(config["joints_rads_low"] * 6)
self.joints_rads_diff = np.array(config["joints_rads_diff"] * 6)
self.joints_rads_high = self.joints_rads_diff + self.joints_rads_low
self.coxa_joint_ids = range(0, 18, 3)
self.femur_joint_ids = range(1, 18, 3)
self.tibia_joint_ids = range(2, 18, 3)
self.left_joints_ids = [0, 1, 2, 6, 7, 8, 12, 13, 14]
self.right_joints_ids = [3, 4, 5, 9, 10, 11, 15, 16, 17]
p.setGravity(0, 0, -9.8, physicsClientId=self.client_ID)
p.setRealTimeSimulation(0, physicsClientId=self.client_ID)
p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=self.client_ID)
self.urdf_name = config["urdf_name"]
self.robot = self.load_robot()
if config["terrain_name"] == "flat":
self.terrain = p.loadURDF("plane.urdf", physicsClientId=self.client_ID)
else:
self.terrain = self.generate_rnd_env()
# Episodal parameters
self.step_ctr = 0
self.episode_ctr = 0
self.episode_rew_list = []
self.rew_mean = 0.
self.rew_std = 1.
self.xd_queue = []
self.joint_work_done_arr_list = []
self.joint_angle_arr_list = []
self.prev_yaw_dev = 0
self.max_dist_travelled = 0
self.target_vel_nn_input = 0
self.prev_act = np.zeros(18)
self.prev_scaled_joint_angles = np.zeros(18)
self.prev_joints = np.zeros(18)
self.difficult_state_queue = []
self.progress = 1
self.added_difficult_state_this_episode = False
self.create_targets()
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model, adds a
motion constraint to the model and defines the cameras of the model.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
Returns:
boolean - True if the method ran correctly, False otherwise
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
for base_link in ["Hip", "Pelvis"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[base_link].getIndex(),
0,
physicsClientId=self.physics_client)
for shoulder_roll_link in ["RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[shoulder_roll_link].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
elif "Wheel" in joint_name:
self.joint_dict.pop(joint_name)
self.camera_top = CameraRgb(self.robot_model,
self.link_dict["CameraTop_optical_frame"],
hfov=56.3,
vfov=43.7,
physicsClientId=self.physics_client)
self.camera_bottom = CameraRgb(
self.robot_model,
self.link_dict["CameraBottom_optical_frame"],
hfov=56.3,
vfov=43.7,
physicsClientId=self.physics_client)
self.camera_depth = CameraDepth(
self.robot_model,
self.link_dict["CameraDepth_optical_frame"],
hfov=58.0,
vfov=45.0,
physicsClientId=self.physics_client)
self.motion_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=[translation[0], translation[1], 0],
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.laser_manager = Laser(self.robot_model,
self.link_dict["Tibia"].getIndex(),
physicsClientId=self.physics_client)
self.base_controller = PepperBaseController(
self.robot_model, [self.linear_velocity, self.angular_velocity],
[self.linear_acceleration, self.angular_acceleration],
self.motion_constraint,
physicsClientId=self.physics_client)
self.goToPosture("StandZero", 1.0)
def evaluate(parameters, realtime, guiEnabled, recordEnabled):
# Clean up old torque lists and angle records
global torqueList0
global toruqeList2
global hoppingHeightList
global pitchAngleList
global rewardList
global connected
global stepRewardList
if recordEnabled:
torqueList0 = []
torqueList2 = []
hoppingHeightList = []
pitchAngleList = []
rewardList = []
if guiEnabled:
p.disconnect()
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
if (len(sys.argv) > 1) and (str(sys.argv[1]) == '-r'):
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.ER_TINY_RENDERER, 0)
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 1)
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
fileName=(str(os.getcwd()) + '/recordedVideos/' +
str(strftime("%Y-%m-%d%H:%M:%S", gmtime())) +
'.mp4'))
else:
if connected:
p.resetSimulation()
else:
p.connect(p.DIRECT)
connected = True
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
robotStartPos = [0, 0, 2.2]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
# obstacle = p.loadURDF("mountain.urdf", [1.5, 0, 0], p.getQuaternionFromEuler([1.57, 0, -1.57]), useFixedBase=1, globalScaling=0.5)
# obstacle = p.loadURDF("/mesh/stair.urdf", [-6, 9.8, 0], p.getQuaternionFromEuler([0, 0, -1.57]), useFixedBase=1, globalScaling=0.5)
# obstacle = p.loadURDF("/mesh/ledder.urdf", [0.5, 0, 0], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=1, globalScaling=2)
# obstacle2 = p.loadURDF("/mesh/ledder.urdf", [3.5, 0, 1], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=1, globalScaling=2)
# obstacle3 = p.loadURDF("/mesh/ledder.urdf", [6.5, 0, 2], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=1, globalScaling=2)
#obstacle = p.loadURDF("/mesh/airLedder.urdf", [0, 0, 0], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=1, globalScaling=2)
robotId = p.loadURDF("3d.urdf",
robotStartPos,
robotStartOrientation,
useFixedBase=0)
robotPos, robotOrn = p.getBasePositionAndOrientation(robotId)
p.setJointMotorControlArray(robotId, [0, 1, 2],
p.POSITION_CONTROL,
targetPositions=[0.0, 0, 0],
forces=[0, 0, 0])
n = 0
energyCost = sum(map(abs, parameters))
parameters = np.repeat(parameters, TimeSlotSteps)
torqueList = list(map(convertLogTorqueToLinearTorque, parameters))
# if guiEnabled:
# plt.plot(parameters)
# plt.title('Raw Signal')
# plt.plot(torqueList)
# plt.title('Sigmoid Magnified')
# plt.show()
# fs = 4000
# cutoff = 50
# B, A = butter(3, cutoff / (fs / 2), btype='low') # 1st order Butterworth low-pass
# lowPassedTorquelist = lfilter(B, A, torqueList, axis=0)
smoothedTorqueList = scipy.ndimage.filters.gaussian_filter1d(torqueList, 1)
# if guiEnabled:
# plt.plot(smoothedTorqueList)
# plt.show()
totalTorqueList = list(split(smoothedTorqueList, N_NUM_ACTUATORS))
t0 = totalTorqueList[0]
t1 = totalTorqueList[1]
totalCost = 0
for i in range(len(t0)):
stepRobotSimulation([t0[i], t1[i]], robotId, planeId, recordEnabled)
# speedCost += sum(map(abs, list(p.getBaseVelocity(robotId)[1])))
positionCost = p.getBasePositionAndOrientation(robotId)[0][0]
# postureCost = p.getEulerFromQuaternion(p.getBasePositionAndOrientation(robotId)[1])[1]
postureCost = abs(p.getBasePositionAndOrientation(robotId)[1][1])
# postureCosty += abs(p.getBasePositionAndOrientation(robotId)[1][1])
heightCost = p.getBasePositionAndOrientation(robotId)[0][2]
# positionCost += abs(p.getBasePositionAndOrientation(robotId)[0][0])
contactPoint = 0
contact = p.getContactPoints(bodyA=robotId, linkIndexA=-1)
if len(contact) > 0:
contactPoint = 1
tempList = heightCost
totalCost += tempList
if recordEnabled:
stepRewardList.append(tempList)
rewardList.append(totalCost)
if recordEnabled:
p.stopStateLogging(0)
return totalCost / len(t0),
def __init__(self,
max_steps=500,
seed=1234,
gui=False,
map_width=4,
chamber_fraction=1/3,
observation_height=15,
iters=2000,
render_shape=(128*2, 128*2, 3),
observation_shape=(64, 64, 3),
obs_fov=60,
obs_aspect=1.0,
obs_nearplane=0.01,
obs_farplane=100,
reset_upon_touch=False,
touch_thresh=1.,
n_particles=5,
):
super(MaxwellsDemonEnv, self).__init__()
self._GRAVITY = -9.8
self._dt = 1/100.0
self.sim_steps = 5
# Temp. Doesn't currently make sense if smaller.
assert(map_width >= 4.)
self._map_area = self._map_width ** 2
# This constant doesn't affect the created width.
self._cube_width = 1.
self.dt = self._dt
self._game_settings = {"include_egocentric_vision": True}
# self.action_space = gym.spaces.Box(low=np.array([-1.2, -1.2, 0]), high=np.array([1.2,1.2,1]))
if gui:
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = pybullet.connect(pybullet.GUI)
MaxwellsDemonEnv.count += 1
else:
self._physicsClient = pybullet.connect(pybullet.DIRECT)
RLSIMENV_PATH = os.environ['RLSIMENV_PATH']
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.resetSimulation()
#pybullet.setRealTimeSimulation(True)
pybullet.setGravity(0,0,self._GRAVITY)
pybullet.setTimeStep(self._dt)
# _planeId = pybullet.loadURDF("plane.urdf", )
pybullet.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 0.5]
cubeStartOrientation = pybullet.getQuaternionFromEuler([0.,0,0])
# These exist as part of the pybullet installation.
self._demon = pybullet.loadURDF(DATA_DIR + "/sphere2_yellow.urdf", cubeStartPos, cubeStartOrientation, useFixedBase=0)
self._particles = []
for _ in range(self._n_particles):
self._particles.append(pybullet.loadURDF("sphere2red.urdf", cubeStartPos, cubeStartOrientation, useFixedBase=0))
pybullet.setCollisionFilterPair(self._demon, self._particles[-1], -1, -1, enableCollision=0)
pybullet.setAdditionalSearchPath(RLSIMENV_PATH + '/rlsimenv/data')
# Add walls
self._blocks = []
self._wall_heights = [0.5, 1.5]
self.action_space = gym.spaces.Box(low=np.array([-5, -5, -10]), high=np.array([5, 5, 10])) #self._wall_heights[-1]]))
# self._wall_heights = (0.5, 1.5)
x_full_right = (1+self._chamber_fraction) * self._map_width + self._cube_width
x_inner_right = self._map_width
for z in self._wall_heights:
cube_locations = []
# Right walls
# cube_locations.extend([[self._map_width, y, z] for y in range(-self._map_width, -2)])
# cube_locations.extend([[self._map_width, y, z] for y in range(2, self._map_width)])
# Right wall
cube_locations.extend([[x_full_right, y, z] for y in np.arange(-self._map_width, self._map_width)])
# Left wall
cube_locations.extend([[-self._map_width, y, z] for y in range(-self._map_width, self._map_width)])
# Top Wall
cube_locations.extend([[x, self._map_width, z] for x in np.arange(-self._map_width, x_full_right + self._cube_width)])
# Bottom Wall
cube_locations.extend([[x, -self._map_width, z] for x in np.arange(-self._map_width, x_full_right + self._cube_width)])
# Add small room
# Add Right wall
# "Small" small room
# cube_locations.extend([[self._map_width+(self._map_width//2), y, z] for y in range(-self._map_width//2, self._map_width//2)])
# cube_locations.extend([[self._map_width, y, z] for y in range(-self._map_width, self._map_width)])
# Top wall
# cube_locations.extend([[x, self._map_width//2, z] for x in range(self._map_width, self._map_width+(self._map_width//2))])
# Bottom wall
# cube_locations.extend([[x, -self._map_width//2, z] for x in range(self._map_width, self._map_width+(self._map_width//2))])
# print ("cube_locations: ", cube_locations)
for loc in cube_locations:
blockId = pybullet.loadURDF(DATA_DIR + "/cube2.urdf", loc, cubeStartOrientation, useFixedBase=1)
self._blocks.append(blockId)
self._doors = []
# door_locations = [[self._map_width, y, 0] for y in range(-2, 2)]
door_locations = [[self._map_width, y, z] for y in range(-self._map_width, self._map_width)]
for loc in door_locations:
blockId = pybullet.loadURDF(DATA_DIR + "/door_cube.urdf", loc, cubeStartOrientation, useFixedBase=1)
self._doors.append(blockId)
pybullet.setCollisionFilterPair(self._demon, self._doors[-1], -1, -1, enableCollision=0)
self._roof = []
for x in np.arange(-self._map_width, x_full_right):
for y in range(-self._map_width, self._map_width):
loc = [x, y, self._wall_heights[-1]+0.5]
self._roof.append(pybullet.loadURDF(DATA_DIR + "/cube2.urdf",
loc,
cubeStartOrientation,
useFixedBase=1))
pybullet.changeVisualShape(self._roof[-1], -1, rgbaColor=[.4, .4, .4, 0.0])
for body in self._particles + [self._demon] + self._doors + self._blocks + self._roof:
pybullet.changeDynamics(body,
-1,
rollingFriction=0.,
spinningFriction=0.0,
lateralFriction=0.0,
linearDamping=0.0,
angularDamping=0.0,
restitution=1.0,
maxJointVelocity=10)
# disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce = 1
for joint in range(pybullet.getNumJoints(self._demon)):
pybullet.setJointMotorControl2(self._demon,joint,pybullet.POSITION_CONTROL,force=jointFrictionForce)
#for i in range(10000):
# pybullet.setJointMotorControl2(botId, 1, pybullet.TORQUE_CONTROL, force=1098.0)
# pybullet.stepSimulation()
#import ipdb
#ipdb.set_trace()
pybullet.setRealTimeSimulation(1)
# lo = self.getObservation()["pixels"] * 0.0
# hi = lo + 1.0
lo = 0.
hi = 1.
self._game_settings['state_bounds'] = [lo, hi]
# self._observation_space = ActionSpace(self._game_settings['state_bounds'])
# self.observation_space = gym.spaces.Box(low=lo, high=hi, shape=(64,64,3))
self.observation_space = gym.spaces.Box(low=lo, high=hi, shape=(64,64,3))
def sim_bullet(self, table, mode='GUI'):
if mode == "GUI":
self.mode = "GUI"
p.connect(
p.GUI, 1234
) # use build-in graphical user interface, p.DIRECT: pass the final results
p.resetDebugVisualizerCamera(
cameraDistance=0.45,
cameraYaw=-90.0,
cameraPitch=-89,
cameraTargetPosition=[1.55, 0.85, 1.4])
elif mode == "DIRECT":
p.connect(p.DIRECT)
else:
print("Input wrong simulation mode ")
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setGravity(0., 0., -9.81)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
path_robots = "/home/hszlyw/PycharmProjects/ahky/robots"
# tablesize [2.14 x 1.22]
file1 = os.path.join(path_robots, "models", "air_hockey_table",
"model.urdf")
# self.table = p.loadURDF(file, [1.7, 0.85, 0.117], [0, 0, 0.0, 1.0])
self.table = p.loadURDF(file1, list(table[:3]), list(table[3:]))
file2 = os.path.join(path_robots, "models", "puck", "model2.urdf")
# self.puck = p.loadURDF(file, puck_poses[0, 0:3], [0, 0, 0.0, 1.0], flags=p.URDF_USE_IMPLICIT_CYLINDER)
self.puck = p.loadURDF(file2,
self.init_pos, [0, 0, 0.0, 1.0],
flags=p.URDF_USE_INERTIA_FROM_FILE
or p.URDF_MERGE_FIXED_LINKS)
j_puck = self.get_joint_map(self.puck)
j_table = self.get_joint_map(self.table)
tableshape = p.getCollisionShapeData(self.table,
j_table.get(b'base_joint'))
puckshape = p.getCollisionShapeData(self.puck, -1)
p.changeDynamics(self.puck, -1, lateralFriction=1)
p.changeDynamics(self.puck, -1, restitution=1)
p.changeDynamics(self.puck, -1, linearDamping=self.damp)
# p.changeDynamics(self.puck, -1, restitution=0.8)
# load parameters
p.changeDynamics(self.table,
j_table.get(b'base_joint'),
lateralFriction=self.t_lateral_f)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_l'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_r'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_top'),
lateralFriction=1) # no collision
p.changeDynamics(self.table,
j_table.get(b'base_left_rim'),
lateralFriction=self.left_rim_f)
p.changeDynamics(self.table,
j_table.get(b'base_right_rim'),
lateralFriction=self.left_rim_f)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_l'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_r'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_top'),
lateralFriction=1) # no collision
p.changeDynamics(self.table,
j_table.get(b'base_joint'),
restitution=0.2)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_l'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_r'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_top'),
restitution=self.four_side_rim_res) # no collision
p.changeDynamics(self.table,
j_table.get(b'base_left_rim'),
restitution=self.left_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_right_rim'),
restitution=self.right_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_l'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_r'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_top'),
restitution=self.four_side_rim_res) # no collision
# init puck
self.collision_filter(self.puck, self.table)
p.resetBasePositionAndOrientation(self.puck, self.init_pos,
[0, 0, 0, 1.])
p.resetBaseVelocity(self.puck,
linearVelocity=self.lin_vel,
angularVelocity=self.ang_vel)
# START
pre_pos = self.init_pos
pos = [pre_pos] # record position
vel = [self.lin_vel] # record velocity
t_sim = [0] # record time
t = 0
p.setTimeStep(1 / 120.)
while (np.abs(np.array(p.getBaseVelocity(self.puck)[0][:-1])) >
.1).any() and .115 < np.abs(
p.getBasePositionAndOrientation(self.puck)[0][2]) < 1.2:
# while True:
p.stepSimulation()
# time.sleep(1/120.)
# t += 1/120
t_sim.append(t)
new_pos = p.getBasePositionAndOrientation(self.puck)[0]
pos.append(new_pos)
vel.append(p.getBaseVelocity(self.puck)[0])
if mode == 'GUI':
p.addUserDebugLine(pre_pos,
new_pos,
lineColorRGB=[0.5, 0.5, 0.5],
lineWidth=3)
pre_pos = new_pos
p.disconnect()
return np.array(t_sim)[:, None], np.array(pos), np.array(vel)
def init(self):
if (self._game_settings['render']):
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = p.connect(p.GUI)
else:
self._physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0, 0, self._GRAVITY)
p.setTimeStep(self._dt)
# p.connect(p.GUI)
RLSIMENV_PATH = os.environ['RLSIMENV_PATH']
p.loadURDF("plane.urdf")
self._agent = p.loadURDF(RLSIMENV_PATH +
"/rlsimenv/data/kinova/jaco_on_table.urdf",
[0, 0, 0.8],
useFixedBase=False)
# gravId = p.addUserDebugParameter("gravity",-10,10,-10)
self._jointIds = []
paramIds = []
self._init_root_vel = p.getBaseVelocity(self._agent)
self._init_root_pos = p.getBasePositionAndOrientation(self._agent)
self._init_pose = []
p.setPhysicsEngineParameter(numSolverIterations=100)
p.changeDynamics(self._agent, -1, linearDamping=0, angularDamping=0)
self._jointAngles = [
0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9, 0, 0, 1.0204, -1.97,
-0.084, 2.06, -1.9, 0
]
activeJoint = 0
self._action_bounds = [[], []]
for j in range(p.getNumJoints(self._agent)):
p.changeDynamics(self._agent, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(self._agent, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC
or jointType == p.JOINT_REVOLUTE):
self._jointIds.append(j)
### Update action bounds
self._action_bounds[0].append(info[8])
self._action_bounds[1].append(info[9])
# print ("self._action_bounds: ", self._action_bounds)
# paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,jointAngles[activeJoint]))
# self._paramIds.append()
p.resetJointState(self._agent, j,
self._jointAngles[activeJoint])
activeJoint += 1
p.setRealTimeSimulation(0)
lo = [0.0 for l in self.getObservation()[0]]
hi = [1.0 for l in self.getObservation()[0]]
state_bounds = [lo, hi]
state_bounds = [state_bounds]
self._game_settings['state_bounds'] = [lo, hi]
self._state_length = len(self._game_settings['state_bounds'][0])
# print ("self._state_length: ", self._state_length)
self._observation_space = ActionSpace(
self._game_settings['state_bounds'])
self._game_settings['action_bounds'] = self._action_bounds
self._action_space = ActionSpace(self._action_bounds)
self._last_state = self.getState()
self._last_pose = p.getBasePositionAndOrientation(self._agent)[0]
def _create_world(self, initialize_goal_image=False):
"""
This function loads the urdfs of the robot in all the pybullet clients
:param initialize_goal_image: (bool) used to indicate if pybullet client
repsonsible for the goal image needs
to be initialized.
:return:
"""
self._reset_world()
finger_base_position = [0, 0, 0.0]
finger_base_orientation = [0, 0, 0, 1]
if initialize_goal_image:
client_list = [
self._pybullet_client_w_o_goal_id,
self._pybullet_client_w_goal_id, self._pybullet_client_full_id
]
else:
client_list = [
self._pybullet_client_w_o_goal_id, self._pybullet_client_full_id
]
for client in client_list:
if client is not None:
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath(),
physicsClientId=client)
pybullet.setGravity(0, 0, -9.81, physicsClientId=client)
pybullet.setTimeStep(self._simulation_time,
physicsClientId=client)
pybullet.loadURDF("plane_transparent.urdf", [0, 0, 0],
physicsClientId=client)
pybullet.loadURDF(fileName=self._finger_urdf_path,
basePosition=finger_base_position,
baseOrientation=finger_base_orientation,
useFixedBase=1,
flags=(pybullet.URDF_USE_INERTIA_FROM_FILE |
pybullet.URDF_USE_SELF_COLLISION),
physicsClientId=client)
if self.link_name_to_index is None:
self.link_name_to_index = {
pybullet.getBodyInfo(WorldConstants.ROBOT_ID,
physicsClientId=client)[0].decode("UTF-8"):
-1,
}
for joint_idx in range(
pybullet.getNumJoints(WorldConstants.ROBOT_ID,
physicsClientId=client)):
link_name = pybullet.getJointInfo(
WorldConstants.ROBOT_ID,
joint_idx,
physicsClientId=client)[12].decode("UTF-8")
self.link_name_to_index[link_name] = joint_idx
self._revolute_joint_ids = [
self.link_name_to_index[name]
for name in WorldConstants.JOINT_NAMES
]
self.finger_tip_ids = [
self.link_name_to_index[name]
for name in WorldConstants.TIP_LINK_NAMES
]
self.finger_link_ids = self._revolute_joint_ids
self.last_joint_position = [0] * len(
self._revolute_joint_ids)
for link_id in self.finger_link_ids:
pybullet.changeDynamics(
bodyUniqueId=WorldConstants.ROBOT_ID,
linkIndex=link_id,
maxJointVelocity=1e3,
restitution=0.8,
jointDamping=0.0,
lateralFriction=0.1,
spinningFriction=0.1,
rollingFriction=0.1,
linearDamping=0.5,
angularDamping=0.5,
contactStiffness=0.1,
contactDamping=0.05,
physicsClientId=client)
self._create_stage(client)
return
########################################################################
# Initialization of PyBullet variables
v_prev = np.matrix(
np.zeros(robot.nv)
).T # velocity during the previous time step, of size (robot.nv,1)
# Start the client for PyBullet
physicsClient = p.connect(p.DIRECT) #or p.DIRECT for non-graphical version
# Set gravity (disabled by default)
#p.setGravity(0,0,-9.81)
# Load horizontal plane
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
# Load Quadruped robot
robotStartPos = [0, 0, 1.0] #0.35
robotStartOrientation = p.getQuaternionFromEuler([0.17, 0.09, 0.0])
#robotStartOrientation = p.getQuaternionFromEuler([0,0,0])
p.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/solo_description/robots")
robotId = p.loadURDF("solo.urdf", robotStartPos, robotStartOrientation)
# Disable default motor control for revolute joints
revoluteJointIndices = [0, 1, 3, 4, 6, 7, 9, 10]
p.setJointMotorControlArray(
robotId,
jointIndices=revoluteJointIndices,
import pybullet as p
from time import sleep
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf", [0, 0, 0])
p.setGravity(0, 0, 0)
arm_height = 5
arm_width = 1
arm_length = 4
body_height = 1
body_width = 3
body_length = 4
# create bodies
mod1 = p.loadURDF("cube_small.urdf", [
body_length / 2 + arm_width + arm_length + 0.25,
body_width / 2 - arm_width / 2, 2.5
],
globalScaling=5)
p.changeVisualShape(mod1, -1, rgbaColor=[1, 0, 0, 1])
mod2 = p.loadURDF("cube_small.urdf", [
body_length / 2 + arm_width + arm_length + 0.25,
-body_width / 2 + arm_width / 2, arm_height / 2
],
globalScaling=5)
def set_minimal_environment():
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("plane.urdf")
pybullet.setGravity(0, 0, -9.81)
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0,0,-9.8)
p.setTimeStep(1./500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
#enable collision between lower legs
for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[]
def __init__(self, **kwargs):
use_gui = utils.get_var_from_kwargs("use_gui",
optional=True,
default=False,
**kwargs)
verbose = utils.get_var_from_kwargs("verbose",
optional=True,
default=False,
**kwargs)
log_file = utils.get_var_from_kwargs("log_file",
optional=True,
default=False,
**kwargs)
use_real_time_simulation = utils.get_var_from_kwargs(
"use_real_time_simulation", optional=True, default=False, **kwargs)
fixed_time_step = utils.get_var_from_kwargs("fixed_time_step",
optional=True,
default=0.01,
**kwargs)
logger_name = utils.get_var_from_kwargs("logger_name",
optional=True,
default=__name__,
**kwargs)
self.CYLINDER = sim.GEOM_CYLINDER
self.BOX = sim.GEOM_BOX
self.MESH = sim.GEOM_MESH
self.logger = logging.getLogger(logger_name + __name__)
utils.setup_logger(self.logger, logger_name, verbose, log_file)
if use_gui:
self.gui = sim.connect(sim.GUI_SERVER)
# self.gui = sim.connect(sim.GUI)
else:
self.gui = sim.connect(sim.DIRECT)
sim.setAdditionalSearchPath(pybullet_data.getDataPath())
self.joint_name_to_id = OrderedDict()
self.joint_id_to_name = OrderedDict()
self.start_state_for_traj_planning = OrderedDict()
self.end_state_for_traj_planning = OrderedDict()
self.scene_items = OrderedDict()
self.joint_name_to_info = OrderedDict()
self.joint_id_to_info = OrderedDict()
self.joint_name_to_jac_id = OrderedDict()
self.ignored_collisions = DefaultOrderedDict(bool)
self.link_pairs = DefaultOrderedDict(list)
self.robot_info = DefaultOrderedDict(list)
self.planning_group = []
self.planning_group_ids = []
self.joint_ids = []
self.planning_samples = 0
self.collision_safe_distance = 0.4
self.collision_check_distance = 0.2
self.collision_check_time = 0
self.robot = None
if use_real_time_simulation:
sim.setRealTimeSimulation(use_real_time_simulation)
else:
sim.setTimeStep(fixed_time_step)
self.collision_constraints = []
