Exemple #1
0
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)
Exemple #4
0
# 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)
Exemple #5
0
    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
Exemple #6
0
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]
Exemple #7
0
    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)
Exemple #8
0
    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])
Exemple #12
0
# 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.)
		
Exemple #15
0
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()
Exemple #16
0
    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)
Exemple #20
0
    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)
Exemple #21
0
            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()
Exemple #23
0
    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)
Exemple #24
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),
Exemple #25
0
    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))
Exemple #26
0
    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]
Exemple #28
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,
Exemple #30
0
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)
Exemple #31
0
def set_minimal_environment():
    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
    pybullet.loadURDF("plane.urdf")
    pybullet.setGravity(0, 0, -9.81)
Exemple #32
0
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 = []