def reset(self): self.ordered_joints = [] print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf)) if self.self_collision: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base, flags=p.URDF_USE_SELF_COLLISION)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base)) self.robot_specific_reset() s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use self.potential = self.calc_potential() return s
def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
def reset(self):
if self.isEnableSelfCollision:
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2], flags=p.URDF_USE_SELF_COLLISION)
else:
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2])
self.kp = 1
self.kd = 1
self.maxForce = 3.5
self.nMotors = 8
self.motorIdList = []
self.motorDir = [-1, -1, -1, -1, 1, 1, 1, 1]
self.buildJointNameToIdDict()
self.buildMotorIdList()
def _randomly_place_objects(self, urdfList):
"""Randomly places the objects in the bin.
Args:
urdfList: The list of urdf files to place in the bin.
Returns:
The list of object unique ID's.
"""
# Randomize positions of each object urdf.
objectUids = []
for urdf_name in urdfList:
xpos = 0.4 +self._blockRandom*random.random()
ypos = self._blockRandom*(random.random()-.5)
angle = np.pi/2 + self._blockRandom * np.pi * random.random()
orn = p.getQuaternionFromEuler([0, 0, angle])
urdf_path = os.path.join(self._urdfRoot, urdf_name)
uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
[orn[0], orn[1], orn[2], orn[3]])
objectUids.append(uid)
# Let each object fall to the tray individual, to prevent object
# intersection.
for _ in range(500):
p.stepSimulation()
return objectUids
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def get_cube(x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z)
p.changeDynamics(body,-1, mass=1.2)#match Roboschool
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
useMaximalCoordinates=maximalCoordinates)
for i in range(p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
for i in range(numObjects):
cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0, 0, -10)
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("%s/plane.urdf" % urdfRoot)
p.setGravity(0, 0, -10)
global minitaur
minitaur = Minitaur(urdfRoot)
start_position = current_position()
last_position = None # for tracing line
total_energy = 0
for i in range(maxNumSteps):
torques = minitaur.getMotorTorques()
velocities = minitaur.getMotorVelocities()
total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
joint_values = evaluate_func_map[evaluateFunc](i, params)
minitaur.applyAction(joint_values)
p.stepSimulation()
if (is_fallen()):
break
if i % 100 == 0:
sys.stdout.write('.')
sys.stdout.flush()
time.sleep(sleepTime)
print(' ')
alpha = objectiveParams[0]
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime
print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
p.setGravity(0,0,-10)
self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def reset(self):
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
self.kp = 1
self.kd = 0.1
self.maxForce = 3.5
self.nMotors = 8
self.motorIdList = []
self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
self.buildJointNameToIdDict()
self.buildMotorIdList()
def test_rolling_friction(self):
import pybullet as p
p.connect(p.DIRECT)
p.loadURDF("plane.urdf")
sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
#p.changeDynamics(sphere,-1,rollingFriction=0)
p.setGravity(0, 0, -10)
for i in range(1000):
p.stepSimulation()
vel = p.getBaseVelocity(sphere)
self.assertLess(vel[0][0], 1e-10)
self.assertLess(vel[0][1], 1e-10)
self.assertLess(vel[0][2], 1e-10)
self.assertLess(vel[1][0], 1e-10)
self.assertLess(vel[1][1], 1e-10)
self.assertLess(vel[1][2], 1e-10)
p.disconnect()
def _reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.2*random.random()
ypos = 0 +0.25*random.random()
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.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def _reset(self):
"""Environment reset called at the beginning of an episode.
"""
# Set the camera settings.
look = [0.23, 0.2, 0.54]
distance = 1.
pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3)
yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3)
roll = 0
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
look, distance, yaw, pitch, roll, 2)
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
aspect = self._width / self._height
near = 0.01
far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
self._attempted_grasp = False
self._env_step = 0
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# Choose the objects in the bin.
urdfList = self._get_random_object(
self._numObjects, self._isTest)
self._objectUids = self._randomly_place_objects(urdfList)
self._observation = self._get_observation()
return np.array(self._observation)
def reset(self):
self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
self.kp = 1
self.kd = 0.1
self.maxForce = 100
nJoints = p.getNumJoints(self.quadruped)
self.jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(self.quadruped, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self.resetPose()
for i in range(100):
p.stepSimulation()
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
minitaur = Minitaur()
amplitude = 0.24795664427
speed = 0.2860877729434
for i in range(1000):
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
minitaur.applyAction(joint_values)
p.stepSimulation()
# print(minitaur.getBasePosition())
time.sleep(timeStep)
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
print(final_distance)
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def __enter__(self):
print("connecting")
optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring,*self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0,0,-10)
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,-10)
sleep(1./240.)
import pybullet as p
import time
import math
from datetime import datetime
from time import sleep
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move.
jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0, 0, 0)
while 1:
p.stepSimulation()
for i in range(1):
pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
import time
import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,0)
print(c)
if (c<0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
minitaur = p.loadURDF("quadruped/minitaur.urdf")
robot_cid = -1
POSITION=1
ORIENTATION=2
BUTTONS=6
p.setRealTimeSimulation(1)
controllerId = -1
while True:
events = p.getVREvents()
for e in (events):
#print (e[BUTTONS])
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED ):
)
args = parser.parse_args()
sim = simulation.Simulation("rrr/robot.urdf", fixed=True, panels=True)
sliders = {}
target = None
joints = sim.getJoints()
bx = 0.07
bz = 0.25
###################################################################################################
###################################################################################################
""" Definitions control sliders """
if args.mode == "direct":
target = p.loadURDF("target2/robot.urdf")
for joint in joints:
sliders[joint] = p.addUserDebugParameter(joint, -math.pi, math.pi, 0.0)
elif args.mode == "inverse":
target = p.loadURDF("target2/robot.urdf")
sliders["target_x"] = p.addUserDebugParameter("target_x", -1, 1, 0.4)
sliders["target_y"] = p.addUserDebugParameter("target_y", -1, 1, 0)
sliders["target_z"] = p.addUserDebugParameter("target_z", -1, 1, 0.25)
elif args.mode == "triangle" or args.mode == "triangle-points":
sliders["triangle_x"] = p.addUserDebugParameter("triangle_x", 0.01, 0.8,
0.4)
sliders["triangle_z"] = p.addUserDebugParameter("triangle_z", -0.2, 0.3, 0)
sliders["triangle_h"] = p.addUserDebugParameter("triangle_h", 0.01, 0.3,
0.1)
sliders["triangle_w"] = p.addUserDebugParameter("triangle_w", 0.01, 0.3,
0.2)
import pybullet as p
import pybullet_data
import math
from statistics import mean
import time
import decimal
from time import sleep
physicsClient = p.connect(p.GUI)
#p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane2.urdf")
#planeId = p.loadSDF("stadium.sdf")
cubeStartPos = [0, 0, 1.5]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF(
"1dHopper_withoutBeam.urdf",
cubeStartPos,
cubeStartOrientation,
useFixedBase=0,
globalScaling=1.0
)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
currentSpringLength = float(0.5)
useRealTimeSimulation = 0
stateArray = ["LOADING", "COMPRESSION", "THRUST", "UNLOADING", "FLIGHT"]
currentState = "NULL"
hasThrusted = False
toeOffTheGround = False
isRecording = False # Marker for stance phase time recording
startTime = time.time()
touchDownTime = time.time()
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
def plot_from_dict(steps, data_dict):
for name, data in data_dict.items():
plt.plot(steps, data, label='%s' % name)
plt.legend()
#plt.show()
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")
#creat a cylinder
test_visual1 = p.createVisualShape(p.GEOM_CYLINDER, radius=0.5, length=0.5)
test_collision1 = p.createCollisionShape(p.GEOM_CYLINDER,
radius=0.5,
height=0.5)
#creat a capsule
test_visual2 = p.createVisualShape(p.GEOM_CAPSULE,
radius=0.1,
length=1,
visualFramePosition=[0, 0, 0.5],
visualFrameOrientation=[0, 0, 0, 1])
test_collision2 = p.createCollisionShape(
p.GEOM_CAPSULE,
def load_urdf(self, urdf):
return pb.loadURDF(urdf)
log = list()
for chunk in chunks:
if len(chunk) == sz:
values = struct.unpack(fmt, chunk)
record = list()
for i in range(ncols):
record.append(values[i])
log.append(record)
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
p.loadURDF("tray/tray.urdf", [0, 0, 0])
p.loadURDF("block.urdf", [0, 0, 2])
log = readLogFile("data/block_grasp_log.bin")
recordNum = len(log)
itemNum = len(log[0])
objectNum = p.getNumBodies()
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
def init_grasp(self):
try:
p.removeBody(self.box_id)
except:
pass
pos_traj = np.load(os.path.join(self.env_root, 'init', 'pos.npy'))
orn_traj = np.load(os.path.join(self.env_root, 'init', 'orn.npy'))
self.fix_orn = np.load(os.path.join(self.env_root, 'init', 'orn.npy'))
for j in range(7):
self.p.resetJointState(self.robotId, j, self.data_q[0][j],
self.data_dq[0][j])
self.robot.gripperControl(0)
for init_t in range(100):
box = p.getAABB(self.obj_id, -1)
center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
center[0] -= 0.05
center[1] -= 0.05
center[2] += 0.03
# center = (box[0]+box[1])*0.5
points = np.array([pos_traj[0], center])
start_id = 0
init_traj = point2traj(points)
start_id = self.move(init_traj, orn_traj, start_id)
# grasping
grasp_stage_num = 10
for grasp_t in range(grasp_stage_num):
gripperPos = grasp_t / float(grasp_stage_num) * 180.0
self.robot.gripperControl(gripperPos)
start_id += 1
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([pos, [pos[0], pos[1] + 0.05, pos[2] + 0.3]])
start_id = self.move(up_traj, orn_traj, start_id)
if self.opt.rand_start == 'rand':
# move in z-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos, [pos[0], pos[1], pos[2] + (random.random() - 0.5) * 0.1]
])
start_id = self.move(up_traj, orn_traj, start_id)
# move in y-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos,
[pos[0], pos[1] + (random.random() - 0.5) * 0.2 + 0.2, pos[2]]
])
start_id = self.move(up_traj, orn_traj, start_id)
# move in x-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos, [pos[0] + (random.random() - 0.5) * 0.2, pos[1], pos[2]]
])
start_id = self.move(up_traj, orn_traj, start_id)
elif self.opt.rand_start == 'two':
prob = random.random()
if prob < 0.5:
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([pos, [pos[0], pos[1] + 0.2, pos[2]]])
start_id = self.move(up_traj, orn_traj, start_id)
if self.opt.rand_box == 'rand':
self.box_file = os.path.join(self.env_root,
"urdf/openbox/openbox.urdf")
self.box_position = [
0.42 + (random.random() - 0.5) * 0.2,
0.00 + (random.random() - 0.5) * 0.4, 0.34
]
self.box_scaling = 0.00037
self.box_orientation = p.getQuaternionFromEuler(
[0, 0, math.pi / 2])
self.box_id = p.loadURDF(fileName=self.box_file,
basePosition=self.box_position,
baseOrientation=self.box_orientation,
globalScaling=self.box_scaling
) #, physicsClientId=self.physical_id)
else:
self.box_file = os.path.join(self.env_root,
"urdf/openbox/openbox.urdf")
self.box_position = [0.42, 0.00, 0.34]
self.box_scaling = 0.00037
self.box_orientation = p.getQuaternionFromEuler(
[0, 0, math.pi / 2])
self.box_id = p.loadURDF(fileName=self.box_file,
basePosition=self.box_position,
baseOrientation=self.box_orientation,
globalScaling=self.box_scaling
) #, physicsClientId=self.physical_id)
texture_path = os.path.join(self.env_root, 'texture/sun_textures')
texture_file = os.path.join(
texture_path,
random.sample(os.listdir(texture_path), 1)[0])
textid = p.loadTexture(texture_file)
# p.changeVisualShape (self.box_id, -1, rgbaColor=[1, 1, 1, 0.9])
# p.changeVisualShape (self.box_id, -1, textureUniqueId=textid)
p.changeVisualShape(self.box_id, -1, rgbaColor=[1, 0, 0, 1])
self.start_pos = p.getLinkState(self.robotId, 7)[0]
box = p.getAABB(self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
obj = p.getAABB(self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])]
self.last_aabb_dist = sum([
(x - y)**2 for x, y in zip(box_center, obj_center)
])**0.5
import serial
import time
import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
#p.resetSimulation()
p.setGravity(0,0,-10)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
def load_object():
boxId = p.loadURDF("object.urdf")
return boxId
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000
# physId = p.connect(p.SHARED_MEMORY_GUI)
if (1):
p.connect(p.GUI)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
# p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
# "genericlogdata.bin",
# maxLogDof=16,
# logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
useFixedBase=False,
# if row[4] == "hand":
# print ("found hand")
# trajectory.append(trajectory_point)
# return trajectory
show_gt = args.gt # using this parameter, the robot shows the behaviour with ground-truth input data this can be seen as current best case scenario
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.GUI)
video_filename = os.path.basename(args.csvfile)[:-4] + ".mp4"
# print("Log video to: ")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
# p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, video_filename)
p.loadURDF(config["environment"]["model"], [0, 0, 0], globalScaling=0.01)
jd = config["jd"]
ll = config["ll"]
ul = config["ul"]
jr = config["jr"]
rest_pose = config["rest_pose"]
robot_name = config["robot"]["name"]
robot_origin_pos = config["robot"]["origin_pos"]
robot_origin_orientation = config["robot"]["origin_orientation"]
robotId = p.loadURDF(config["robot"]["model"], robot_origin_pos,
robot_origin_orientation)
robotEndeffectorIndex = config["robot"]["endeffector_index"]
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
window.addItem(grid)
global_axis = gl.GLAxisItem()
global_axis.updateGLOptions({'glLineWidth': (4, )})
window.addItem(global_axis)
print("WINDOW not updated")
window.update()
# configure pybullet and load plane.urdf and quadcopter.urdf
physicsClient = p.connect(
p.DIRECT) # pybullet only for computations no visualisation
p.setGravity(0, 0, -gravity)
p.setTimeStep(Tsample_physics)
# disable real-time simulation, we want to step through the
# physics ourselves with p.stepSimulation()
p.setRealTimeSimulation(0)
planeId = p.loadURDF("plane.urdf", [0, 0, 0],
p.getQuaternionFromEuler([0, 0, 0]))
quadcopterId = p.loadURDF("quadrotor.urdf", [0, 0, 1],
p.getQuaternionFromEuler([0, 0, 0]))
# do a few steps to start simulation and let the quadcopter land safely
for i in range(int(2 / Tsample_physics)):
p.stepSimulation()
# create a pyqtgraph mesh from the quadcopter to visualize
# the quadcopter in the 3D pyqtgraph window
quadcopterMesh = bullet2pyqtgraph(quadcopterId)[0]
window.addItem(quadcopterMesh)
window.update()
# Initialize PID controller gains:
Kp = np.zeros(6)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]]
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0],
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1],
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]]
return rayFrom,rayTo
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0,-0.02,0]
meshScale=[0.1,0.1,0.1]
vertices=[
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,1.000000,1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
chunks = wholeFile.split(b'\xaa\xbb')
log = list()
for chunk in chunks:
if len(chunk) == sz:
values = struct.unpack(fmt, chunk)
record = list()
for i in range(ncols):
record.append(values[i])
log.append(record)
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
p.loadURDF("tray/tray.urdf",[0,0,0])
p.loadURDF("block.urdf",[0,0,2])
log = readLogFile("data/block_grasp_log.bin")
recordNum = len(log)
itemNum = len(log[0])
objectNum = p.getNumBodies()
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
for objectId in range(objectNum):
import pybullet as p
import pybullet_data
import time
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.setGravity(0,0,-10)
planeId = p.loadURDF("road_plain.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
p.stepSimulation()
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos,cubeOrn)
time.sleep(5)
p.disconnect()
controls = {}
robotPath = "phantomx_description/urdf/phantomx.urdf"
sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False)
# sim.setFloorFrictions(lateral=0, spinning=0, rolling=0)
pos, rpy = sim.getRobotPose()
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
leg_center_pos = [0.1248, -0.06164, 0.001116 + 0.5]
leg_angle = -math.pi / 4
params = Parameters()
if args.mode == "frozen-direct":
crosses = []
for i in range(4*6):
crosses.append(p.loadURDF("target2/robot.urdf"))
for name in sim.getJoints():
print(name)
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi, 0)
elif args.mode == "direct":
for name in sim.getJoints():
print(name)
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi, 0)
elif args.mode == "inverse":
cross = p.loadURDF("target2/robot.urdf")
# Use your own DK function
alphas = kinematics.computeDK(0, 0, 0, use_rads=True)
controls["target_x"] = p.addUserDebugParameter("target_x", -0.4, 0.4, alphas[0])
controls["target_y"] = p.addUserDebugParameter("target_y", -0.4, 0.4, alphas[1])
CONTROLLER_ID = 0
POSITION=1
ORIENTATION=2
NUM_MOVE_EVENTS=5
BUTTONS=6
ANALOG_AXIS=8
#assume that the VR physics server is already started before
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS
colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
widths = [3]*p.VR_MAX_CONTROLLERS
#use a few default colors
colors[0] = [0,0,0]
colors[1] = [0.5,0,0]
colors[2] = [0,0.5,0]
colors[3] = [0,0,0.5]
colors[4] = [0.5,0.5,0.]
colors[5] = [.5,.5,.5]
controllerId = -1
pt=[0,0,0]
p.setJointMotorControl2(bodyIndex=robot,
jointIndex=knee_motor_id,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=standstilltorque)
p.setJointMotorControl2(bodyIndex=robot,
jointIndex=spring_motor_id,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=standstilltorque)
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")
robotStartPos = [0, 0, 0.23]
robotStartOrientation = p.getQuaternionFromEuler([np.pi / 2, 0, 0])
robot = p.loadURDF("/home/pramod/Single_leg_test/Urdf/Test_1/urdf/Test_1.urdf",
robotStartPos,
robotStartOrientation,
useFixedBase=1)
# ResetLeg()
a = 0.06
b = 0.03
x_path = []
z_path = []
y_path = []
for j in range(0, 361, 5):
# Ellipse
x = -0.06 + a * m.cos(m.radians(j))
import pybullet
import time
import numpy as np #to reshape for matplotlib
plt.ion()
img = [[1,2,3]*50]*100#np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
pybullet.connect(pybullet.DIRECT)
#pybullet.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.setGravity(0,0,-10)
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
pitch = -10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
import pybullet as p
import time
import math
useGui = True
if (useGui):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.loadURDF("samurai.urdf")
p.loadURDF("r2d2.urdf",[3,3,1])
rayFrom=[]
rayTo=[]
numRays = 1024
rayLen = 13
for i in range (numRays):
rayFrom.append([0,0,1])
rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1])
if (not useGui):
timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json")
BUTTONS = 6
import pybullet_data
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
p.setGravity(0, 0, -10)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf")
car = p.loadURDF(
"racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range(p.getNumJoints(car)):
print(p.getJointInfo(car, i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
p.getJointInfo(car, wheel)
wheels = [8, 15]
import pybullet as p
import pybullet_data
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c < 0):
c = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setInternalSimFlags(0) #don't load default robot assets etc
p.resetSimulation()
#p.resetSimulation()
p.setGravity(0, 0, -10)
objects = [
p.loadURDF("plane.urdf", 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
0.000000, 1.000000)
]
objects = [
p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000,
0.707107, 0.000000, 0.707107)
]
objects = [
p.loadURDF("jenga/jenga.urdf", 1.200000, -0.700000, 0.750000, 0.000000,
0.707107, 0.000000, 0.707107)
]
objects = [
p.loadURDF("jenga/jenga.urdf", 1.100000, -0.700000, 0.750000, 0.000000,
0.707107, 0.000000, 0.707107)
]
objects = [
import pybullet as p
p.connect(p.GUI)
cube = p.loadURDF("cube.urdf")
frequency = 240
timeStep = 1. / frequency
p.setGravity(0, 0, -9.8)
p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
for i in range(frequency):
p.stepSimulation()
pos, orn = p.getBasePositionAndOrientation(cube)
print(pos)
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.addUserDebugText("tip", [0, 0, 0.1],
textColorRGB=[1, 0, 0],
textSize=1.5,
parentObjectUniqueId=kuka,
parentLinkIndex=6)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=kuka,
parentLinkIndex=6)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=kuka,
parentLinkIndex=6)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=kuka,
parentLinkIndex=6)
p.setRealTimeSimulation(0)
angle = 0
while (True):
time.sleep(0.01)
p.resetJointState(kuka, 2, angle)
p.resetJointState(kuka, 3, angle)
angle += 0.01
from mykinematics import *
from config_parse import *
import time
import numpy as np
direct = p.connect(p.GUI)
delta = 0.001
c1 = 0.23
c2 = -0.3084
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0, 0, -9.81)
# p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setAdditionalSearchPath('models')
p.loadURDF('floor/floor.urdf')
arm = p.loadURDF('dof.urdf')
# for i in range(p.getNumJoints(arm)):
# print(p.getJointInfo(arm, i))
# print('')
# print([x[0] for x in p.getJointStates(arm, [0,1,2,3,4])])
def rot_to_quat(phi):
R = get_rot_matrix_from_euler(0, phi, 0)
tr = R[0, 0] + R[1, 1] + R[2, 2]
if tr > 0.0:
S = np.sqrt(tr + 1.0) * 2
qw = 0.25 * S
qx = (R[2, 1] - R[1, 2]) / S
def f(params):
dic_params = {
pname: param
for pname, param in zip(param_names, params)
}
offset = 0.01 if object_name == "yball" else 0.0
init_poses = {
"rake": {
"push": np.array([0.0, 0.0, 0.0, -0.04 - offset, 0.0]),
"draw": np.array([0.0, 0.0, 0.0, 0.055 + offset, 0.025]),
"tap_from_right":
np.array([0.0, 0.0, 0.0, 0.03, -0.145 - offset]),
"tap_from_left": np.array([0.0, 0.0, 0.0, 0.02, 0.14 + offset])
},
"stick": {
"push": np.array([0.0, 0.0, 0.0, -0.035 - offset, 0.0]),
"draw": np.array([0.0, 0.0, 0.0, 0.035 + offset, 0.03]),
"tap_from_right":
np.array([0.0, 0.0, 0.0, 0.06, -0.065 - offset]),
"tap_from_left": np.array([0.0, 0.0, 0.0, 0.06, 0.05 + offset])
},
"hook": {
"push": np.array([0.0, 0.0, 0.0, -0.04 - offset, 0.0]),
"draw": np.array([0.0, 0.0, 0.0, 0.15 + offset, 0.05]),
"tap_from_right":
np.array([0.0, 0.0, 0.0, 0.06, -0.075 - offset]),
"tap_from_left": np.array([0.0, 0.0, 0.0, 0.09, 0.10 + offset])
}
}
costs = list()
sim_eff_history = np.zeros((N_EXPERIMENTS, 2))
for tool_name in tools:
for action_name in actions:
target_pos, target_var, gnd_weight, mdist, real_eff_history = load_experiment(
"affordance-datasets/visual-affordances-of-objects-and-tools/{}/{}/{}/effData.txt"
.format(tool_name, object_name, action_name),
get_eff_data=True)
for iter in range(N_EXPERIMENTS):
success = False
while not success:
try:
if WATCHDOG:
signal.alarm(10)
with stdout_redirected():
robotID = load_robot(tool_name)
toolID = robotID[0]
nonlocal objID
if (toolID == objID):
raise ValueError
mu = init_poses[tool_name][action_name]
yaw, pitch, roll, x, y = np.random.normal(
mu, np.array([1.0, 1.0, 1.0, 0.0, 0.0]))
initial_xy = np.array([x, y])
p.resetBasePositionAndOrientation(
objID,
posObj=[x, y, 0.05],
ornObj=[yaw, pitch, roll, 1])
dic_params2 = dict(dic_params)
dic_params2['linearDamping'] = 0.0
dic_params2['angularDamping'] = 0.0
#dic_params2['mass']=0.001*models[object_name][1]
#dic_params2['rollingFriction']=0.00001
#dic_params['lateralFriction']=0.0001
#dic_params['spinningFriction']=0.0001
#dic_params['rollingFriction']=0.0001
#del dic_params2['xnoise']
#del dic_params2['ynoise']
p.changeDynamics(objID, -1, **dic_params2)
get_obj_xy(objID)
speed = 0.04
if action_name == "push":
base_speed = [-speed, 0, 0]
base_pos_limit = lambda js: js[0] <= -0.12
elif action_name == "draw":
base_speed = [speed, 0, 0]
base_pos_limit = lambda js: js[0] >= 0.12
elif action_name == "tap_from_left":
base_speed = [0, speed, 0]
base_pos_limit = lambda js: js[1] >= 0.12
elif action_name == "tap_from_right":
base_speed = [0, -speed, 0]
base_pos_limit = lambda js: js[1] <= -0.12
else:
raise ValueError
# Let object fall to the ground and stop it
pxyz, pori = p.getBasePositionAndOrientation(objID)
position_after_fall = 100 * np.ones_like(pxyz)
orientation_after_fall = 100 * np.ones_like(pori)
while not np.allclose(position_after_fall[-1:],
pxyz[-1:],
atol=1e-6):
p.stepSimulation()
pxyz = position_after_fall
position_after_fall, orientation_after_fall = p.getBasePositionAndOrientation(
objID)
p.resetBasePositionAndOrientation(
objID,
posObj=[
initial_xy[0], initial_xy[1],
position_after_fall[2]
],
ornObj=orientation_after_fall)
p.resetBaseVelocity(objID, [0.0, 0.0, 0.0])
ppos = get_obj_xy(objID)
npos = 100 * np.ones_like(ppos)
iters = 0
# Move tool
p.resetBaseVelocity(toolID, base_speed)
action_finnished = False
while not np.allclose(npos, ppos,
atol=1e-6) or iters < 100:
js, jor = p.getBasePositionAndOrientation(
toolID)
if (base_pos_limit(js)):
p.resetBaseVelocity(toolID, [0, 0, 0])
action_finnished = True
elif not action_finnished:
p.resetBaseVelocity(toolID, base_speed)
p.stepSimulation()
ppos = npos
npos = get_obj_xy(objID)
if action_finnished:
iters += 1
# time.sleep(1./1000.)
pos = npos
delete_robot(toolID)
sim_eff_history[iter] = pos - initial_xy
success = True
except ValueError:
p.resetSimulation()
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # optionally
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(enableFileCaching=0)
planeId = p.loadURDF("plane.urdf")
p.changeDynamics(planeId,
-1,
restitution=.97,
linearDamping=0.0,
angularDamping=0.0)
objID = load_object()
kld = KLD(real_eff_history, sim_eff_history)
if PLOTTING:
plt.scatter(real_eff_history[:, 1],
-real_eff_history[:, 0],
s=40,
c="red",
edgecolors='none',
label="real")
plt.scatter(sim_eff_history[:, 1],
-sim_eff_history[:, 0],
s=40,
c="blue",
edgecolors='none',
label="sim")
plt.legend(loc=2)
plt.ylim(-0.3, 0.3)
plt.xlim(-0.3, 0.3)
plt.title('action: {}, tool: {}'.format(
action_name, tool_name))
plt.xlabel('[m]')
plt.ylabel('[m]')
plt.savefig("{}.png".format(kld))
plt.savefig("{}.pdf".format(kld))
plt.show()
costs.append(kld)
sim_eff_history = np.zeros((N_EXPERIMENTS, 2))
logging.info("cost:{}".format(kld))
if WATCHDOG:
signal.alarm(0)
out = sum(costs) / len(costs)
print('\033[93m' + str(dic_params) + '\033[0m')
pbar.set_description('cost: %0.2f' % (out))
pbar.update(1)
return out
def init_decor(client):
"""
Creation of the different elements in the environment
"""
global x1, x2, x3
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("/decor/table/table.urdf",
basePosition=[3, 0, 0.30],
globalScaling=10.0,
physicsClientId=client)
pybullet.loadURDF("/decor/ball/totemball.urdf",
basePosition=[2.7, x1, 0.65],
physicsClientId=client)
pybullet.loadURDF("/decor/teddy/totemteddybear.urdf",
basePosition=[2.7, x2, 0.65],
physicsClientId=client)
pybullet.loadURDF("/decor/bird/totembird.urdf",
basePosition=[2.7, x3, 0.65],
physicsClientId=client)
pybullet.loadURDF("/decor/bed/bed.urdf",
basePosition=[0.3, -2.9, 1],
physicsClientId=client)
pybullet.loadURDF("/decor/box/box.urdf",
basePosition=[0.5, 0.5, 0.19],
physicsClientId=client)
pybullet.loadURDF("/decor/chair/chair.urdf",
basePosition=[3, -2, 0.3],
physicsClientId=client)
pybullet.loadURDF("/decor/night/night.urdf",
basePosition=[0, -2, 0.2],
physicsClientId=client)
pybullet.loadURDF("/decor/floor/floor.urdf",
basePosition=[3, 0, 0.01],
globalScaling=10.0,
physicsClientId=client)
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=[]
import pybullet as p
usePort = True
if (usePort):
id = p.connect(p.GRPC, "localhost:12345")
else:
id = p.connect(p.GRPC, "localhost")
print("id=", id)
if (id < 0):
print("Cannot connect to GRPC server")
exit(0)
print("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf")
print("numJoints = ", p.getNumJoints(r2d2))
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
p.setGravity(0,0,-10)
sleep(0.01) # Time in seconds.
else:
p.stepSimulation()
def __init__(self,
files,
actuation_spec,
observation_spec,
gamma,
horizon,
timestep=1 / 240,
n_intermediate_steps=1,
debug_gui=False,
**viewer_params):
"""
Constructor.
Args:
files (list): Paths to the URDF files to load;
actuation_spec (list): A list of tuples specifying the names of the
joints which should be controllable by the agent and tehir control mode.
Can be left empty when all actuators should be used in position control;
observation_spec (list): A list containing the names of data that
should be made available to the agent as an observation and
their type (ObservationType). An entry in the list is given by:
(name, type);
gamma (float): The discounting factor of the environment;
horizon (int): The maximum horizon for the environment;
timestep (float, 0.00416666666): The timestep used by the PyBullet
simulator;
n_intermediate_steps (int): The number of steps between every action
taken by the agent. Allows the user to modify, control and
access intermediate states;
**viewer_params: other parameters to be passed to the viewer.
See PyBulletViewer documentation for the available options.
"""
# Store simulation parameters
self._timestep = timestep
self._n_intermediate_steps = n_intermediate_steps
# Create the simulation and viewer
if debug_gui:
pybullet.connect(pybullet.GUI)
else:
pybullet.connect(pybullet.DIRECT)
pybullet.setTimeStep(self._timestep)
pybullet.setGravity(0, 0, -9.81)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
self._viewer = PyBulletViewer(
self._timestep * self._n_intermediate_steps, **viewer_params)
self._state = None
# Load model and create access maps
self._model_map = dict()
for file_name, kwargs in files.items():
model_id = pybullet.loadURDF(file_name, **kwargs)
model_name = pybullet.getBodyInfo(model_id)[1].decode('UTF-8')
self._model_map[model_name] = model_id
self._model_map.update(self._custom_load_models())
self._joint_map = dict()
self._link_map = dict()
for model_id in self._model_map.values():
for joint_id in range(pybullet.getNumJoints(model_id)):
joint_data = pybullet.getJointInfo(model_id, joint_id)
if joint_data[2] != pybullet.JOINT_FIXED:
joint_name = joint_data[1].decode('UTF-8')
self._joint_map[joint_name] = (model_id, joint_id)
link_name = joint_data[12].decode('UTF-8')
self._link_map[link_name] = (model_id, joint_id)
# Read the actuation spec and build the mapping between actions and ids
# as well as their limits
assert (len(actuation_spec) > 0)
self._action_data = list()
for name, mode in actuation_spec:
if name in self._joint_map:
data = self._joint_map[name] + (mode, )
self._action_data.append(data)
low, high = self._compute_action_limits()
action_space = Box(np.array(low), np.array(high))
# Read the observation spec to build a mapping at every step. It is
# ensured that the values appear in the order they are specified.
if len(observation_spec) == 0:
raise AttributeError(
"No Environment observations were specified. "
"Add at least one observation to the observation_spec.")
else:
self._observation_map = observation_spec
# We can only specify limits for the joint positions, all other
# information can be potentially unbounded
low, high = self._compute_observation_limits()
observation_space = Box(low, high)
# Finally, we create the MDP information and call the constructor of
# the parent class
mdp_info = MDPInfo(observation_space, action_space, gamma, horizon)
super().__init__(mdp_info)
# Save initial state of the MDP
self._initial_state = pybullet.saveState()
import pybullet as p
import time
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
p.connect(p.DIRECT)
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
print("plugin=",plugin)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf",[0,0,-1])
p.loadURDF("r2d2.urdf")
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camDistance = 4
pitch = -10.0
roll=0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
float(mouseX) * dHor[1] - float(mouseY) * dVer[1],
rayFrom[2] + rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2] +
float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,
"visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("./pybullet_data/plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName="duck.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
import pybullet as p
from time import sleep
import matplotlib.pyplot as plt
import numpy as np
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
import pybullet as p
import math
import numpy as np
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf", [0, 0, 1])
def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
)
camPos = [
camTarget[0] - dist * camForward[0],
camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]),
(camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0] * rayForward[0] +
rayForward[1] * rayForward[1] +
rayForward[2] * rayForward[2])
invLen = farPlane * 1. / lenFwd
rayForward = [
invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]
]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
PERIOD = np.array([2.0, 2.0, 2.0]) # sec
PHASE = np.array([-np.pi / 4, 0, 0])
else:
if not INVERSE:
PERIOD = np.array([2.0, 2.0, 2.0]) # sec
PHASE = np.array([0, 2 * np.pi / 3, -2 * np.pi / 3])
else:
PERIOD = np.array([2.0, 2.0, 2.0]) # sec
PHASE = np.array([0, -2 * np.pi / 3, 2 * np.pi / 3])
timestep = 0
# set env.
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
snakeStartPos = [0, 0, 0.5]
snakeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
snakeId = p.loadURDF("./urdf/trident_snake.urdf", snakeStartPos,
snakeStartOrientation)
p.changeDynamics(bodyUniqueId=snakeId, linkIndex=2, lateralFriction=1)
p.changeDynamics(bodyUniqueId=snakeId, linkIndex=5, lateralFriction=1)
p.changeDynamics(bodyUniqueId=snakeId, linkIndex=7, lateralFriction=1)
# enable joint F/T sensor
num_joints = p.getNumJoints(snakeId)
for i in range(num_joints):
p.enableJointForceTorqueSensor(snakeId, i)
def get_joint_data_array(body_id, joint_id):
import pybullet as p
draw = 1
printtext = 0
if (draw):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
r2d2 = p.loadURDF("r2d2.urdf")
def drawAABB(aabb):
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMax[0], aabbMin[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 0, 0])
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMin[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [0, 1, 0])
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMin[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [0, 0, 1])
f = [aabbMin[0], aabbMin[1], aabbMax[2]]
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMin[0], aabbMin[1], aabbMax[2]]
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMin[1], aabbMin[2]]
import pybullet as p
import time
import pybullet_data
import pybullet_envs
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # to load plane.urdf
p.loadURDF("plane.urdf")
# p.loadURDF("urdf/plane.urdf",basePosition=[0,0,-1.5])
# humanoid = p.loadURDF("cassie/urdf/cassie_collide.urdf",[0,0,0.8], useFixedBase=False)
# humanoid = p.loadURDF("urdf/simbicon_urdf/biped2d.urdf",[0, 0, 1.2])
# humanoid = p.loadURDF("urdf/simbicon_urdf/humanoid_nohead.urdf",[0, 0, 0.31])
# humanoid = p.loadURDF("urdf/simbicon_urdf/flame.urdf",[0, 0, 1.0])
humanoid = p.loadURDF("urdf/simbicon_urdf/flame3.urdf", [0, 0, 0.9])
# humanoid = p.loadURDF("urdf/simbicon_urdf/demo.urdf")
# gravId = p.addUserDebugParameter("gravity",-10,10,-10)
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []
test_visual = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.2, 1, 0.1],
rgbaColor=[1, 0, 0, 1])
test_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 1, 0.1])
test_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=test_collision, \
baseVisualShapeIndex=test_visual, basePosition = [-0.15, 0, 0])
# shift = [0, -0.02, 0]
# meshScale = [0.1, 0.1, 0.1]
# visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,
