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 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):
# 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):
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):
# 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 _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 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 main():
global writer
if CREATE_ANIMATION == 1:
if os.path.isfile(gifPath):
os.remove(gifPath)
writer = imageio.get_writer(gifPath, mode='I')
FRAME_COUNTER = 0
pybulletPath = "C:/Users/SBWork/Documents/pythonLibs/bullet3/data/"
tablePath = pybulletPath + "table/table_mid.urdf"
kukaPath = pybulletPath + "kuka_lwr/kuka.urdf"
#kukaPath = pybulletPath + "kuka_iiwa/kuka_world.sdf";
jengaPath = pybulletPath + "jenga/jenga_mid2.urdf"
gripperPath = pybulletPath + "gripper/wsg50_one_motor_gripper_new_free_base.sdf"
#gripperPath = pybulletPath + "gripper/wsg50_one_motor_gripper.sdf"
ballPath = pybulletPath + "sphere_small.urdf"
client = p.connect(p.GUI)
p.setTimeStep(1.0 / SIM_SECOND)
p.setGravity(0.0, 0.0, -9.8)
p.resetDebugVisualizerCamera(5, 40, 0, [-.0376, 0.3159, -.0344])
TABLE_HEIGHT = .62 * 2
tableID = p.loadURDF(tablePath)
robot0_initial_pos = [-1.2, 0, TABLE_HEIGHT]
robot1_initial_pos = [1.2, 0, TABLE_HEIGHT]
tower_initial_pos = [0, 0, TABLE_HEIGHT]
kuka0ID = p.loadURDF(kukaPath, robot0_initial_pos, useFixedBase=True)
kuka1ID = p.loadURDF(kukaPath, robot1_initial_pos, useFixedBase=True)
#jenga1ID = p.loadURDF(jengaPath,[1,1,1]);
#kukaID = p.loadSDF(kukaPath,0);
#ballID = p.loadURDF(ballPath,[.7,0,TABLE_HEIGHT]);
for i in range(0, p.getNumJoints(kuka0ID)):
p.setJointMotorControl2(kuka0ID,
i,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1)
p.setJointMotorControl2(kuka1ID,
i,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1)
gripper0ID = p.loadSDF(gripperPath)[0]
gripper1ID = p.loadSDF(gripperPath)[0]
#cid = p.createConstraint(tableID,-1,kuka0ID,-1,p.JOINT_POINT2POINT,[0,0,0],[0,0,0],[0,0,0]);
#p.changeConstraint(cid,maxForce=10000000);
cid = p.createConstraint(kuka0ID, 6, gripper0ID, -1, p.JOINT_FIXED,
[0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2])
p.changeConstraint(cid, maxForce=10000000)
cid = p.createConstraint(kuka1ID, 6, gripper1ID, -1, p.JOINT_FIXED,
[0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2])
p.changeConstraint(cid, maxForce=10000000)
p.setRealTimeSimulation(enableRealTimeSimulation=1)
#for i in range(1,int(round(SIM_SECOND*.02))):
# moveSim();
#for i in range(0,6):
# print('On link %d'%(i));
# moveLink(kukaID,i);
jengaBlockList = buildTower(jengaPath, 3, 10, [0, 0, TABLE_HEIGHT + .05])
p.setRealTimeSimulation(enableRealTimeSimulation=1)
raw_input()
p.setJointMotorControl2(kuka0ID,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=PI / 2,
positionGain=1)
p.setJointMotorControl2(kuka0ID,
1,
controlMode=p.POSITION_CONTROL,
targetPosition=-PI / 2,
positionGain=1)
p.setJointMotorControl2(kuka1ID,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=PI / 2,
positionGain=1)
p.setJointMotorControl2(kuka1ID,
1,
controlMode=p.POSITION_CONTROL,
targetPosition=-PI / 2,
positionGain=1)
print('A')
for i in range(1, int(round(SIM_SECOND * .02))):
moveSim()
p.setJointMotorControl2(kuka0ID,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1)
p.setJointMotorControl2(kuka0ID,
1,
controlMode=p.POSITION_CONTROL,
targetPosition=-PI / 2,
positionGain=1)
p.setJointMotorControl2(kuka1ID,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=PI,
positionGain=1)
p.setJointMotorControl2(kuka1ID,
1,
controlMode=p.POSITION_CONTROL,
targetPosition=-PI / 2,
positionGain=1)
print('B')
for i in range(1, int(round(SIM_SECOND * .2))):
moveSim()
print('C')
closeGripper(gripper0ID, time=1)
closeGripper(gripper1ID, time=1)
p.setJointMotorControl2(kuka0ID,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1)
p.setJointMotorControl2(kuka0ID,
1,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1,
force=10000)
p.setJointMotorControl2(kuka1ID,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1)
p.setJointMotorControl2(kuka1ID,
1,
controlMode=p.POSITION_CONTROL,
targetPosition=0,
positionGain=1,
force=10000)
print('D')
for i in range(1, int(round(SIM_SECOND * .2))):
moveSim()
def reset(self):
self._init = True
self.done = False
self.steps_taken = 0
self._time_elapsed = 0
self._old_dist_travelled = 0
self._death_wall_pos = -92
if np.random.rand() < self.settings['RANDOM_DISABLE_CHANCE']:
self._random_disable_idx = np.random.choice(
[x for x in range(0, 18, 3)])
else:
self._random_disable_idx = None
# during testing there is no point in making the episodes last different
# times
if self.c_args.mode == 'train':
# frames should be used when these default times differ
self._episode_length = np.random.uniform(
self.settings['DEFAULT_MIN_TIME'],
self.settings['DEFAULT_MAX_TIME'])
else:
self._episode_length = np.inf
# render doesn't have to be called in direct mode by user but it should
# still run at least once
if self.render_env is None:
self.render(mode='direct')
# if the robot is loaded we only need to move it not add it again
if self.robot.robot_body is not None:
self.robot.robot_body.reset_position(self._original_position)
self._original_orientation[2] = np.random.uniform(-0.5, 0.5)
self.robot.robot_body.reset_orientation(self._original_orientation)
self._prev_position = self._original_position
self._reset_joints()
else:
p.resetSimulation(self.client)
p.setGravity(0, 0, -10)
plane_id = p.loadURDF("plane.urdf")
#p.changeDynamics(plane_id, -1, lateralFriction=10)
self.robot.reset(p)
# see above how these are used
self._original_position = self.robot.robot_body.get_position()
self._prev_position = self._original_position
self._original_orientation = list(
self.robot.robot_body.get_orientation())
# move camera closer to robot
p.resetDebugVisualizerCamera(1.2, -145, -38, [0, 0, 0])
p.setTimeStep(self._time_step, self.client)
obs_size = self.settings['STORED_OBSERVATION_SIZE']
prev_obs = self.settings['PREV_OBS_ON_INPUT']
self.obs_buffer = deque(np.zeros(obs_size) for _ in range(prev_obs))
if self.settings['PID_ENABLED']:
self.pid_regs = []
p_params = self.settings['PID_PARAMS']
for idx in range(self.settings['NUM_JOINTS']):
self.pid_regs.append(PID(*p_params, self._time_step))
self.state = self._get_state()
return self.state
import pybullet as p
import time
import math
import sys
sys.path.append(".")
from minitaur import Minitaur
p.connect(p.GUI)
p.setTimeOut(5)
#p.setPhysicsEngineParameter(numSolverIterations=50)
p.setGravity(0,0,-10)
p.setTimeStep(0.01)
urdfRoot = ''
p.loadURDF("%s/plane.urdf" % urdfRoot)
minitaur = Minitaur(urdfRoot)
while (True):
p.stepSimulation()
time.sleep(0.01)
import pybullet_data
import numpy as np
from gym_ergojr import get_scene
from gym_ergojr.utils.urdf_helper import URDF
# Create the bullet physics engine environment
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=0.45,
cameraYaw=135,
cameraPitch=-45,
cameraTargetPosition=[0, 0, 0])
p.setGravity(0, 0, -10) # good enough
frequency = 100 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
# This loads the checkerboard background
planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
# Robot model starting position
startPos = [0, 0, 0] # xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# The robot needs to be defined in a URDF model file. But the model file is clunky, so there is a macro language, "XACRO",
# that can be compiled to URDF but that's easier to read and that contains if/then/else statements, variables, etc.
# This next part looks for XACRO files with the given name and then force-recompiles it to URDF
# ("force" because if there is already a URDF file with the right name, it usually doesn't recompile - this is just for development)
xml_path = get_scene("ergojr-sword")
import pybullet as p
import time
import math
p.connect(p.SHARED_MEMORY)
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("/home/evangeloit/Desktop/py-msc/bullet3-2.88/data/plane100.urdf",
useMaximalCoordinates=True)
# disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# disable tiny renderer, 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]
rangex = 1
rangey = 2
for i in range(rangex):
for j in range(rangey):
# the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=
"/home/evangeloit/Desktop/py-msc/Blender_models/Inclined_Project/Single_Items/Tropical_Wood_Box.obj",
def generate_dataset(self):
# combined dataset for graph classification (GC)
combined_dataset_name = dataset_prefix
combined_dataset_dir = self.save_dir / combined_dataset_name
pathlib.Path(combined_dataset_dir).mkdir(parents=True, exist_ok=True)
combined_node_id = 0
combined_graph_id = 0
combined_graphnode2id = {}
combined_a = open(
combined_dataset_dir / str(combined_dataset_name + "_A.txt"), "w")
combined_graph_indicator = open(
combined_dataset_dir /
str(combined_dataset_name + "_graph_indicator.txt"),
"w",
)
combined_graph_labels = open(
combined_dataset_dir /
str(combined_dataset_name + "_graph_labels.txt"), "w")
combined_node_attributes = open(
combined_dataset_dir /
str(combined_dataset_name + "_node_attributes.txt"),
"w",
)
for num_brick in tqdm(
range(self.min_num_brick, self.max_num_brick + 1),
"Number of Brick Progress",
):
# separate dataset for graph classification (GC)
dataset_name = self.dataset_prefix + "_" + str(num_brick)
dataset_dir = self.save_dir / dataset_name
pathlib.Path(dataset_dir).mkdir(parents=True, exist_ok=True)
node_id = 0
graphnode2id = {}
a = open(dataset_dir / str(dataset_name + "_A.txt"), "w")
graph_indicator = open(
dataset_dir / str(dataset_name + "_graph_indicator.txt"), "w")
graph_labels = open(
dataset_dir / str(dataset_name + "_graph_labels.txt"), "w")
node_attributes = open(
dataset_dir / str(dataset_name + "_node_attributes.txt"), "w")
for episode in tqdm(range(self.num_episode), "Episode Progress"):
# use test_simulator.py for debugging and visualization
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1.0 / 60.0)
# generate new design
self.designer.clear()
self.designer.generate_design(num_brick=num_brick)
# update combined graph_id
graph_id = episode + 1
combined_graph_id += 1
init_pos_list = []
final_pos_list = []
for i, b in enumerate(self.designer.built):
# write to separate GC dataset
node_id += 1
graphnode2id[str(graph_id) + "_" + str(i)] = node_id
graph_indicator.write(str(graph_id) + "\n")
node_attributes.write(",".join(
str(i) for i in b.bounding.flatten().tolist()) + "\n")
# write to combined GC dataset
combined_node_id += 1
combined_graphnode2id[str(combined_graph_id) + "_" +
str(i)] = combined_node_id
combined_graph_indicator.write(
str(combined_graph_id) + "\n")
combined_node_attributes.write(",".join(
str(i) for i in b.bounding.flatten().tolist()) + "\n")
pos = b.anchor
rot = p.getQuaternionFromEuler(
np.array(b.rotation) * np.pi / 2)
brickID = p.loadURDF("urdf/brick.urdf", pos, rot)
init_pos_list.append(pos)
p.loadURDF("plane.urdf", useMaximalCoordinates=True)
p.setGravity(0, 0, -10)
for frame in range(self.num_frame):
p.stepSimulation()
time.sleep(1.0 / 240.0)
if frame == self.num_frame - 1:
for brickID in len(num_brick):
pos, _ = p.getBasePositionAndOrientation(brickID)
final_pos_list.append(pos)
for edge in self.designer.G.edges:
# write to separate GC dataset
row = graphnode2id[str(graph_id) + "_" + str(edge[0])]
col = graphnode2id[str(graph_id) + "_" + str(edge[1])]
a.write(str(row) + ", " + str(col) + "\n")
a.write(str(col) + ", " + str(row) + "\n")
# write to combined GC dataset
combined_row = combined_graphnode2id[str(combined_graph_id)
+ "_" + str(edge[0])]
combined_col = combined_graphnode2id[str(combined_graph_id)
+ "_" + str(edge[1])]
combined_a.write(
str(combined_row) + ", " + str(combined_col) + "\n")
combined_a.write(
str(combined_col) + ", " + str(combined_row) + "\n")
label = int(
self.determine_stable(init_pos_list, final_pos_list))
graph_labels.write(str(label) + "\n")
combined_graph_labels.write(str(label) + "\n")
p.disconnect()
a.close()
graph_indicator.close()
graph_labels.close()
node_attributes.close()
shutil.make_archive(self.save_dir / dataset_name, "zip",
dataset_dir)
shutil.rmtree(dataset_dir)
combined_a.close()
combined_graph_indicator.close()
combined_graph_labels.close()
combined_node_attributes.close()
shutil.make_archive(self.save_dir / combined_dataset_name, "zip",
combined_dataset_dir)
shutil.rmtree(combined_dataset_dir)
filename = rospkg.RosPack().get_path("pybullet_sandbox") + "/data/mug/mug.urdf"
boxId = p.loadURDF(filename)
box_pose = p.getBasePositionAndOrientation(boxId)
sphere_path = rospkg.RosPack().get_path(
"pybullet_sandbox") + "/data/sphere.urdf"
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
# sphere packing algorithm taken from wiki: https://en.wikipedia.org/wiki/Close-packing_of_equal_spheres
r = 0.002
p1 = 5
d = 3
print 4 * p1 * p1 * (d)
for i in range(-p1, p1):
for j in range(-p1, p1):
for k in range(0, d):
x = r * (2 * i + (j + k) % 2)
y = r * sqrt(3) * (j + (k % 2) / 3.0)
z = r * sqrt(6) * 2 * k / 3.0
p.loadURDF(sphere_path, [x, y, z + 0.01])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setTimeStep(0.010)
print "starting sim"
while True:
p.stepSimulation(physicsClient)
minAABB, maxAABB = p.getAABB(boxId)
print len(p.getOverlappingObjects(minAABB, maxAABB))
p.disconnect()
# J = cloudpickle.load(open("jacobian.pkl", "rb"))
# T = cloudpickle.load(open("transform.pkl", "rb"))
# T_euler = cloudpickle.load(open("euler_angles.pkl", "rb"))
class Side(Enum):
LEFT = 0
RIGHT = 1
if SIMULATION == True:
physicsClient = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
p.setGravity(0, 0, -9.81)
p.setTimeStep(1 / CONTROL_FREQUENCY)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
planeId = p.loadURDF("plane.urdf")
bipedId = p.loadURDF("urdf/blackbird_biped.urdf", [0, 0, 1.2],
p.getQuaternionFromEuler([0, 0, math.pi / 2]))
def startup_sequence():
# Wait 7 seconds for IMUs to warm-start
# Get all IMU angles and transform into joint space
# Determine joint angles from closest encoder reading out of 8 possible positions
# Change to standing pose
# Start main control loop once contact is detected on both feet
if SIMULATION == True:
p.setJointMotorControlArray(bipedId,
range(10),
def create_rocket(self, height=1.0, radius=1.):
path = os.path.abspath(os.path.dirname(__file__))
if self.mode == "balance":
p.setGravity(0, 0, -300.8)
self.dry_weight = 10.
self.fuel = 10.0
self.max_thrust = [100.,100.,0.]
shift = [0,0, 1.]
# adjust reward function
self.survival_bonus = 1.0
self.land_bonus = 0.0
self.crash_bonus = 0.0
self.timeout_bonus = 0.0
self.max_steps = 1000
orientation = p.getQuaternionFromEuler(\
[np.random.randn(1) / 15, np.random.randn(1) / 15, np.random.rand(1) / 15])
self.bot_id = p.loadURDF(os.path.join(path, "balance_rocket.xml"),\
shift,\
orientation)
self.plane_id = p.loadURDF("plane.urdf")
p.changeDynamics(self.bot_id, -1, mass = 3.0)
p.changeDynamics(self.bot_id, 1, mass = 1.0)
p.changeDynamics(self.bot_id, 0, mass = self.dry_weight + self.fuel)
# get rid of damping
num_links = 11
for ii in range(num_links):
p.changeDynamics(self.bot_id, ii, angularDamping=0.0, linearDamping=0.0)
self.kg_per_kN = 0.30 / 240 # g/(kN*s). 240 is due to the 240 Hz time step in the physics simulatorchangeDynamicsp.change
elif self.mode == "lunar":
p.setGravity(0,0.,-1.625)
self.start_height = 1.0
self.dry_weight = 3.
min_height = 1.0
self.fuel = 16.0
self.max_thrust = [30., 30., 300.]
# adjust reward function
self.survival_bonus = 0.001
self.land_bonus = 200.0
self.crash_bonus = -300.0
self.timeout_bonus = -350.0
self.max_steps = 1000
# not ready yet
#self.plane_id = p.loadURDF(os.path.join(path,"lunar.urdf"))
self.plane_id = p.loadURDF("plane.urdf")
shift = [0., 0., min_height ]
orientation = p.getQuaternionFromEuler(\
[0.95, 0.0, 0.0])
self.bot_id = p.loadURDF(os.path.join(path, "lander.xml"), shift, orientation)
p.changeDynamics(self.bot_id, 0, mass=self.dry_weight + self.fuel)
p.changeDynamics(self.bot_id, 1, mass=2.0)
for ii in range(2,11):
p.changeDynamics(self.bot_id, 1, mass=00.0)
num_links = 11
for ii in range(num_links):
p.changeDynamics(self.bot_id, ii, angularDamping=0.0, linearDamping=0.0)
self.kg_per_kN = 0.30 / 240 # g/(kN*s). 240 is due to the 240 Hz time step in the physics simulatorchangeDynamicsp.change
else:
p.setGravity(0, 0, -9.8)
self.start_height = 4.0
self.dry_weight = 1.
self.fuel = 100.0
self.max_thrust = [10,10.,5000.]
self.start_height = 4.
shift = [0,0, self.start_height]
# adjust reward function
self.survival_bonus = 0.001
self.land_bonus = 200.0
self.crash_bonus = -300.0
self.timeout_bonus = -350.0
self.max_steps = 1000
orientation = p.getQuaternionFromEuler(\
[np.random.randn(1) / 15, np.random.randn(1) / 5, np.random.rand(1) / 3])
self.plane_id = p.loadURDF("plane.urdf")
self.bot_id = p.loadURDF(os.path.join(path, "booster.xml"),\
shift,\
orientation)
p.changeDynamics(self.bot_id, -1, mass = 3.0)
p.changeDynamics(self.bot_id, 1, mass = 1.0)
p.changeDynamics(self.bot_id, 0, mass = self.dry_weight + self.fuel)
# get rid of damping
num_links = 11
for ii in range(num_links):
p.changeDynamics(self.bot_id, ii, angularDamping=0.0, linearDamping=0.0)
self.kg_per_kN = 0.30 / 240 # g/(kN*s). 240 is due to the 240 Hz time step in the physics simulatorchangeDynamicsp.change
p.setTimeStep(0.01)
self.step_count = 0
self.velocity_0 = 0.1
p.changeVisualShape(cube_objects[0], -1, rgbaColor=[1, 1, 1, 1])
p.changeVisualShape(cube_objects[0], -1, textureUniqueId=texUid)
p.resetBasePositionAndOrientation(cube_objects[0],
(0.5000000, 0.60000, 0.6700),
(0.717, 0.0, 0.0, 0.717))
# print("active_joints_info::",active_joints_info)
# print("finger::",finger)
num_joints = p.getNumJoints(fingerID)
# print("`num of joints:::",num_joints)
blockUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "block.urdf"),
xpos, ypos, zpos, orn[0], orn[1], orn[2], orn[3])
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0])
for i in range(num_joints - 1):
j_info = p.getJointInfo(fingerID, i)
# print("joint_info::",j_info)
p.setTimeStep(1.0 / 1000.0)
p.setRealTimeSimulation(0)
link_name = "lbr_iiwa_link_7"
link_name_encoded = link_name.encode(encoding='UTF-8', errors='strict')
kuka_ee_link_jointInfo = jointInfo.searchBy(key="linkName",
value=link_name_encoded)[0]
#print(kuka_ee_link_jointInfo)
kuka_ee_link_Index = 6 #kuka_ee_link_jointInfo["jointIndex"]
# print("kuka_ee_link_Index",kuka_ee_link_Index)
blockPos, blockOrn = p.getBasePositionAndOrientation(blockUid)
#print(blockPos,blockOrn)
new_pos = [xpos, ypos, zpos]
new_orn = [orn[0], orn[1], orn[2], orn[3]]
#print("test1",new_pos)
#print("test2",new_orn)
jointPoses = p.calculateInverseKinematics(fingerID, kuka_ee_link_Index,
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),
})
# TODO(ayzaan): Delete below and uncomment vector box bounds.
position_bounds = gym.spaces.Box(low=-1.0,
high=1.0,
shape=(3, ),
dtype=np.float32)
# 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((position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32))),
'pose1':
gym.spaces.Tuple((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, p.CNSFileIO],
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)
def generate_dataset(self):
for num_brick in tqdm(
range(self.min_num_brick, self.max_num_brick + 1),
"Number of Brick Progress",
):
# separate dataset for GN-based physics engine (GN Physics)
dataset_name = self.dataset_prefix + "_" + str(num_brick)
nodes_train, nodes_eval = [], []
edges_train, edges_eval = [], []
node_feats_train, node_feats_eval = [], []
edge_feats_train, edge_feats_eval = [], []
split = int(self.num_episode * self.train_eval_split)
for episode in tqdm(range(self.num_episode), "Episode Progress"):
if episode < split:
nodes_dummy = nodes_train
edges_dummy = edges_train
node_feats_dummy = node_feats_train
edge_feats_dummy = edge_feats_train
episode_dummy = copy.deepcopy(episode)
else:
nodes_dummy = nodes_eval
edges_dummy = edges_eval
node_feats_dummy = node_feats_eval
edge_feats_dummy = edge_feats_eval
episode_dummy = copy.deepcopy(episode) - split
# use test_simulator.py for debugging and visualization
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1.0 / 60.0)
# generate new design
self.designer.clear()
self.designer.generate_design(num_brick=num_brick)
for b in self.designer.built:
pos = b.anchor
rot = p.getQuaternionFromEuler(
np.array(b.rotation) * np.pi / 2)
p.loadURDF("urdf/brick.urdf", pos, rot)
p.loadURDF("plane.urdf", useMaximalCoordinates=True)
# start simulation
p.setGravity(0, 0, -10)
for frame in range(self.num_frame):
p.stepSimulation()
time.sleep(1.0 / 240.0)
# remember the ground!
for objectID in range(num_brick + 1):
pos, ort = p.getBasePositionAndOrientation(objectID)
rot = p.getEulerFromQuaternion(ort)
vel_linear, vel_angular = p.getBaseVelocity(objectID)
if objectID < num_brick:
mass = 1.0
else:
mass = 9999999.0
nodes_dummy.append([episode_dummy, frame, objectID])
node_feats_dummy.append(pos + rot + vel_linear +
vel_angular + (mass, ))
contact_list = p.getContactPoints()
for contact in contact_list:
edges_dummy.append(
[episode_dummy, frame, contact[1], contact[2]])
edge_feats_dummy.append(contact[5] # positionA
+ contact[6] # positionB
+ (
contact[9], # normalForce
contact[10], # friction1
contact[12], # friction2
))
p.disconnect()
np.savez(
self.save_dir / str(dataset_name + "_TRAIN"),
nodes=np.array(nodes_train),
node_feats=np.array(node_feats_train),
edges=np.array(edges_train),
edge_feats=np.array(edge_feats_train),
)
np.savez(
self.save_dir / str(dataset_name + "_EVAL"),
nodes=np.array(nodes_eval),
node_feats=np.array(node_feats_eval),
edges=np.array(edges_eval),
edge_feats=np.array(edge_feats_eval),
)
plot = True
import time
if (plot):
import matplotlib.pyplot as plt
import math
verbose = False
# Parameters:
robot_base = [0., 0., 0.]
robot_orientation = [0., 0., 0., 1.]
delta_t = 0.0001
# Initialize Bullet Simulator
id_simulator = bullet.connect(bullet.GUI) # or bullet.DIRECT for non-graphical version
bullet.setTimeStep(delta_t)
# Switch between URDF with/without FIXED joints
with_fixed_joints = True
if with_fixed_joints:
id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
else:
id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0,0,-10)
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
basePos,baseOrn = p.getBasePositionAndOrientation(pole)
import pybullet as p
import pybullet_data
import os
import time
GRAVITY = -9.8
dt = 1e-3
iters = 2000
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0, 0, GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
#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(p.getNumJoints(botId)):
p.setJointMotorControl2(botId, joint, p.POSITION_CONTROL, force=jointFrictionForce)
#for i in range(10000):
# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
# p.stepSimulation()
#import ipdb
#ipdb.set_trace()
import time
],
useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
n_objects += 1
t_objects = time.clock() - t0
t_object = t_objects / float(n_objects)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
p.setTimeStep(0.005)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
t0 = time.clock()
n_steps = 1000
for t in xrange(n_steps):
p.stepSimulation()
t_simulation = time.clock() - t0
t_step = t_simulation / float(n_steps)
total = t_simulation + t_object
p.stopStateLogging(logId)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
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,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
def reset(self):
"""Environment reset called at the beginning of an episode.
"""
# Set the camera settings.
# look = [0.1, 0., 0.44]
# distance = 0.8
# pitch = -90
# yaw = -90
# roll = 180
#[0.4317558029454219, 0.1470448842904527, 0.2876218894185256]#[0.23, 0.2, 0.54] # from where the input is
# set 1
# look = [0.1, -0.3, 0.54]
# distance = 1.0
# pitch = -45 + self._cameraRandom * np.random.uniform(-3, 3)
# yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3)
# roll = 145
# pos_range = [0.3, 0.7, -0.2, 0.3]
# set 2
# look = [-0.3, -0.8, 0.54]
# distance = 0.7
# pitch = -28 + self._cameraRandom * np.random.uniform(-3, 3)
# yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3)
# roll = 180
# pos_range = [0.1, 0.8, -0.45, 0.15]
# set 3
look = [0.4, 0.1, 0.54]
distance = 2.0
pitch = -90
yaw = -90
roll = 180
pos_range = [0.3, 0.7, -0.2, 0.3]
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])
self.tableUid = p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.640000,
0.000000, 0.000000, 0.0, 1.0)
p.setGravity(0, 0, -10)
# self._tm700 = tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# num of objs to be placed
while len(self.model_paths) != 0:
urdfList = []
num_obj = random.randint(1, 3)
for i in range(num_obj):
urdfList.append(self.model_paths.pop())
print('img {img_cnt}, {n_obj} objects'.format(img_cnt=self.img_save_cnt+1, n_obj=len(urdfList)))
self._objectUids = self._randomly_place_objects(urdfList, pos_range)
self._observation = self._get_observation()
self.img_save_cnt += 1
for uid in self._objectUids:
p.removeBody(uid)
if self.img_save_cnt == 2:
raise Exception('stop')
return np.array(self._observation)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
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,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
axes1[i].plot(np.arange(num),
acc_ERRs[:, p],
label=label[p],
color=color[i])
if __name__ == "__main__":
bag = rosbag.Bag('./rosbag_data/2020-12-04-12-41-02.bag')
puck_poses, _, _, t = read_bag(bag)
p.connect(
p.GUI, 1234
) # use build-in graphical user interface, p.DIRECT: pass the final results
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(1 / 240)
p.setGravity(0., 0., -9.81)
p.resetDebugVisualizerCamera(cameraDistance=0.45,
cameraYaw=-90.0,
cameraPitch=-89,
cameraTargetPosition=[1.55, 0.85, 1.])
file = os.path.join(os.path.dirname(os.path.abspath(path_robots)),
"models", "air_hockey_table", "model.urdf")
table = p.loadURDF(file, [1.7, 0.85, 0.117], [0, 0, 0.0, 1.0])
file = os.path.join(os.path.dirname(os.path.abspath(path_robots)),
"models", "puck", "model.urdf")
puck = p.loadURDF(file, puck_poses[0, 0:3], [0, 0, 0.0, 1.0])
# for linkidx in range(8):
# p.changeDynamics(table, linkidx, spinningFriction=0.01)
print(p.getJointInfo(turtle, j))
forward = 0
turn = 0
force_applied = False
# logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "forward_sensor.txt")
step = 0
true_x, true_y, true_z = [0], [0], [0]
vel_x, vel_y, vel_z = [0], [0], [0]
mean_x, mean_y = [0], [0]
while p.isConnected():
# p.setRealTimeSimulation(1)
# time.sleep(1.)
# NEED TO USE STEP SIMULATION
p.stepSimulation()
p.setTimeStep(simulation_step)
step = step + 1
print(f'----- Simulation Step {step} -----')
if not force_applied:
p.applyExternalForce(elastic_ball,
-1, [15, 7.5, 0],
p.getBasePositionAndOrientation(elastic_ball)[0],
flags=p.WORLD_FRAME)
force_applied = True
ball_x, ball_y, ball_z = get_ball_position(elastic_ball)
print(f'True ball X: {ball_x}')
print(f'True ball Y: {ball_y}')
print(f'True ball Z: {ball_z}')
flags = p.URDF_ENABLE_SLEEPING
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
r2d2 = -1
for k in range (5):
for i in range (5):
r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags)
#enable sleeping: you can pass the flag during URDF loading, or do it afterwards
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range (p.getNumJoints(r2d2)):
p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0)
print("r2d2=",r2d2)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
timestep = 1./240.
p.setTimeStep(timestep)
p.setGravity(0,0,-10)
while p.isConnected():
p.stepSimulation()
time.sleep(timestep)
#force the object to wake up
p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_WAKE_UP)
def reset(self):
# reset is called once at initialization of simulation
self.vt = 0
self.vd = 0
self.maxV = 24.6 # 235RPM = 24,609142453 rad/sec
self._envStepCounter = 0
p.resetSimulation(0)
self.gravId = p.setGravity(0, 0, -9.8) # m/s^2
p.setTimeStep(0.01) # sec
# p.setGravity(0, 0, -9.8)
# p.setRealTimeSimulation(0)
# initialize ground
plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, plane)
# initialize snake position config
cubeStartPos = [0, 0, 0.001]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
path = os.path.abspath(os.path.dirname(__file__))
if self.snake_urdf is not None:
self._botId = p.loadURDF(
os.path.join(path, self.snake_urdf), # "snakebot_simple.xml"
cubeStartPos,
cubeStartOrientation)
else:
self._botId = self.create_robot()
anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(self.botId,
-1,
lateralFriction=2,
anisotropicFriction=anistropicFriction)
self.jointIds = []
self.paramIds = []
# joint dynamics initialization
for i in range(p.getNumJoints(self.botId)):
p.getJointInfo(self.botId, i)
p.changeDynamics(self.botId,
i,
lateralFriction=2,
anisotropicFriction=anistropicFriction)
info = p.getJointInfo(self.botId, i)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC
or jointType == p.JOINT_REVOLUTE):
self.jointIds.append(i)
self.paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -1, 1,
0.))
p.resetJointState(self.botId, i, 0.)
p.addUserDebugParameter(b"amplitude".decode("utf-8"), -1, 1, 0.)
p.addUserDebugParameter(b"frequency".decode("utf-8"), 0, 1, 0.)
p.addUserDebugParameter(b"alpha".decode("utf-8"), 0, 1, 0.)
p.addUserDebugParameter(b"offset".decode("utf-8"), -1, 1, 0.)
# you *have* to compute and return the observation from reset()
self._observation = self._compute_observation()
self._obs_buffer.append(self._observation)
return np.array(self._observation)
def reset(self):
if (self.test_phase):
global test_steps, test_done
if (test_steps != 0):
self.save_data_test()
test_steps = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
useFixedBase=True)
# Load robot
self._panda = pandaEnv(self._urdfRoot,
timeStep=self._timeStep,
basePosition=[0, 0, 0.625],
useInverseKinematics=self._useIK,
action_space=self.action_dim,
includeVelObs=self.includeVelObs)
# Load table and object for simulation
tableId = p.loadURDF(os.path.join(self._urdfRoot,
"franka_description/table.urdf"),
useFixedBase=True)
table_info = p.getVisualShapeData(tableId, -1)[0]
self._h_table = table_info[5][2] + table_info[3][2]
#limit panda workspace to table plane
self._panda.workspace_lim[2][0] = self._h_table
# Randomize start position of object and target.
#we take the target point
if (self.fixedPositionObj):
if (self.object_position == 0):
#we have completely fixed position
self.obj_pose = [0.6, 0.1, 0.64]
self.target_pose = [0.4, 0.45, 0.64]
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
elif (self.object_position == 1):
#we have completely fixed position
self.obj_pose = [
np.random.uniform(0.5, 0.6),
np.random.uniform(0, 0.1), 0.64
]
self.target_pose = [0.4, 0.45, 0.64]
# self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
elif (self.object_position == 2):
#we have completely fixed position
self.obj_pose = [
np.random.uniform(0.4, 0.6),
np.random.uniform(0, 0.2), 0.64
]
self.target_pose = [
np.random.uniform(0.3, 0.5),
np.random.uniform(0.35, 0.55), 0.64
]
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
elif (self.object_position == 3):
print("")
else:
self.obj_pose, self.target_pose = self._sample_pose()
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
#useful to see where is the taget point
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
if self.type_physics == 1:
# Randomizing the physics of the object...
self.currentMass = np.random.uniform(0.1, 0.8)
self.currentFriction = np.random.uniform(0.1, 0.7)
self.currentDamping = np.random.uniform(0.01, 0.2)
p.changeDynamics(self._objID,
linkIndex=-1,
mass=self.currentMass,
lateralFriction=self.currentFriction,
linearDamping=self.currentDamping)
# Randomizing the physics of the robot... (only joints damping and controller gains)
for i in range(7):
p.changeDynamics(self._panda.pandaId,
linkIndex=i,
linearDamping=np.random.uniform(0.25, 20))
elif self.type_physics == 2:
# Randomizing the physics of the object...
self.currentMass = 0.8
self.currentFriction = 0.2
self.currentDamping = 0.2
p.changeDynamics(self._objID,
linkIndex=-1,
mass=self.currentMass,
lateralFriction=self.currentFriction,
linearDamping=self.currentDamping)
# Randomizing the physics of the robot... (only joints damping and controller gains)
for i in range(7):
p.changeDynamics(self._panda.pandaId,
linkIndex=i,
linearDamping=0.25)
self._debugGUI()
p.setGravity(0, 0, -9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
#we take the dimension of the observation
return self.getExtendedObservation()
#!/usr/bin/env python3
import pybullet as p
from time import sleep
TIME_STEP = 0.001
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(TIME_STEP)
planeId = p.loadURDF("URDF/plane.urdf", [0, 0, 0])
RobotId = p.loadURDF("URDF/gankenkun.urdf", [0, 0, 0])
for joint in range(p.getNumJoints(RobotId)):
p.setJointMotorControl(RobotId, joint, p.POSITION_CONTROL, 0)
index = {
p.getBodyInfo(RobotId)[0].decode('UTF-8'): -1,
}
for id in range(p.getNumJoints(RobotId)):
index[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = id
p.setJointMotorControl(RobotId, index['left_lower_arm_link'],
p.POSITION_CONTROL, -0.5)
p.setJointMotorControl(RobotId, index['right_lower_arm_link'],
p.POSITION_CONTROL, -0.5)
p.setJointMotorControl(RobotId, index['left_upper_arm_link'],
p.POSITION_CONTROL, -0.2)
p.setJointMotorControl(RobotId, index['right_upper_arm_link'],
p.POSITION_CONTROL, -0.2)
def __init__(self,
gui=False,
urdf='pexod.urdf',
dt=1e-3,
control_dt=0.001):
self.GRAVITY = -9.8
self.dt = dt
self.control_dt = control_dt
self.t = 0
if gui:
self.physicsClient = p.connect(p.GUI)
else:
self.physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
p.setGravity(0, 0, self.GRAVITY)
p.setTimeStep(self.dt)
self.planeId = p.loadURDF("plane.urdf")
#Size of the leg links
self.l1 = 0.1
self.l2 = 0.2
#import modified minitaur to include motor_velocity_limit and torque_max
import pybulletminitaur_derpy as minit_derpy
self.mini = minit_derpy.MinitaurDerpy(
p,
os.path.dirname(os.path.abspath(__file__)) + "/bullet3/data/",
motor_velocity_limit=3.14 * 2,
torque_max=3.5)
#bullet links number corresponding to the legs
self.leg_link_ids = [3, 9, 16, 22]
self.descriptor = {3: [], 9: [], 16: [], 22: []}
self.covered_distance = 0
self.init_angles = self.mini.GetMotorAngles()
#Corridor boolean value is true if the minitaur is inside a 1meter wide corridor
self.inside_corridor = True
self.position = self.mini.GetBasePosition()
self.euler = self.mini.GetTrueBaseRollPitchYaw()
#Enable the turnover safety : stops the simulation if pitch rool angles are too high
self.safety_turnover = True
self.angle_lim = 30
self.energy_reward = []
self.forward_reward = 0
self.drift_reward = 0
self.reward = 0
self.currents = []
self.meanTorques = [0, 0, 0, 0, 0, 0, 0, 0]
self.torqueDescriptors = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.tmpTorqueDescriptors = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.descCount = 0
self.meanCount = 0
self.meanTorquesDescriptors = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.meanLegTorques = [0, 0, 0, 0]
self.cycle_index = 0
self.meanCurrents = []
self.dtCurrent = 0
self.safetyCurrentCut = True
[0]])
return X_d, dX_d, d2X_d
def main():
env = StandardEnvironment()
if __name__ == "__main__":
SIM_COMPLETE = False
physicsClient = p.connect(p.GUI) # p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -1 * G)
p.setTimeStep(Tsample_physics)
p.setRealTimeSimulation(0)
cubeStartPos = [1.0, 1.0, 1.0]
cubeStartOrientation = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
quadId = p.loadURDF(uav_model, cubeStartPos, cubeStartOrientation)
planeId = p.loadURDF("plane.urdf")
controller = UAVController(cubeStartPos, cubeStartOrientation)
i = 0
while i < 10: # SIM_COMPLETE is not True:
t = Tsample_physics * i
xyz_pos, orient = p.getBasePositionAndOrientation(quadId)
linearV, angularV = p.getBaseVelocity(quadId)
action = controller.GetActionFT(xyz_pos, orient, linearV, angularV)
for c in range(len(vector)):
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * numJoints)
import pybullet as p
import pybullet_data
import os
import time
GRAVITY = -9.8
dt = 1e-3
iters = 2000
import pybullet_data
p.setAdditionalSearchPath(pybullet_data.getDataPath())
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0, 0, GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos,
cubeStartOrientation)
#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(p.getNumJoints(botId)):
p.setJointMotorControl2(botId,
joint,
p.POSITION_CONTROL,
force=jointFrictionForce)
import pybullet_data
from pkg_resources import parse_version
import os
from deepx import *
import cv2
from otter.gym.bullet import kinova
_urdfRoot = pybullet_data.getDataPath()
_robot_urdfRoot = '../otter/gym/bullet/assets/urdf'
robot_init_pos = [-7.81, 3.546, 12.883, 0.833, -2.753, 4.319, 5.917, 1, 1, 1]
p.connect(p.GUI)
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(0.01)
p.loadURDF(os.path.join(_urdfRoot, "plane.urdf"), [0, 0, -1])
tableUid = p.loadURDF(os.path.join(_urdfRoot, "table/table.urdf"),
[0.0000000, -0.50000, -.620000],
[0.000000, 0.000000, 0.0, 1.0])
cubeUid = p.loadURDF(os.path.join(_robot_urdfRoot, "Cube.urdf"),
[0.0, -0.55, 0.15], [0, 0, 0, 1],
useFixedBase=True)
p.setGravity(0, 0, -9.81)
# camera configuration
xpos = 0
ypos = -0.55 - 0.12 * random.random()
ang = math.pi / 2
def generate_cartpole_data(num_samples, torque_control=False):
# physicsClient = pb.connect(pb.GUI)
physicsClient = pb.connect(pb.DIRECT)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
cartpole_id = pb.loadURDF('hybrid_cartpole.urdf',
flags=pb.URDF_USE_SELF_COLLISION)
pb.setGravity(0, 0, -9.8)
pb.changeDynamics(cartpole_id, 1, restitution=restitution)
pb.changeDynamics(cartpole_id, 2, restitution=restitution)
pb.changeDynamics(cartpole_id, 3, restitution=restitution)
pb.setTimeStep(sim_time_step)
viewMatrix = pb.computeViewMatrix(cameraEyePosition=[0, -3, .5],
cameraTargetPosition=[0, 0, .5],
cameraUpVector=[0, 0, 1])
projectionMatrix = pb.computeProjectionMatrixFOV(fov=45.0,
aspect=1.0,
nearVal=0.1,
farVal=3.1)
num_step = int(data_time_step / sim_time_step)
X = np.zeros((num_samples, 6, width, height), dtype=np.uint8)
X_next = np.zeros((num_samples, 6, width, height), dtype=np.uint8)
U = np.zeros((num_samples, 1), dtype=np.float64)
k = 0
while k < num_samples:
q0 = np.random.rand(2) * (q_up - q_lo) + q_lo
v0 = np.random.rand(2) * (v_up - v_lo) + v_lo
u0 = 2 * (np.random.rand() - 1) * force_mag
for i in range(len(q0)):
pb.resetJointState(cartpole_id, i, q0[i], v0[i])
if torque_control:
pb.setJointMotorControl2(cartpole_id,
1,
pb.TORQUE_CONTROL,
force=u0)
else:
pb.setJointMotorControl2(cartpole_id,
1,
pb.VELOCITY_CONTROL,
force=0)
# check don't start with overlap already
aabb_pole_min, aabb_pole_max = pb.getAABB(cartpole_id, 1)
overlaps = pb.getOverlappingObjects(aabb_pole_min, aabb_pole_max)
if overlaps is not None and len(overlaps) > 0:
if (0, 2) in overlaps or (0, 3) in overlaps:
continue
width0, height0, rgb0, depth0, seg0 = pb.getCameraImage(
width=width,
height=height,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
for i in range(num_step):
pb.stepSimulation()
width1, height1, rgb1, depth1, seg1 = pb.getCameraImage(
width=width,
height=height,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
for i in range(num_step):
pb.stepSimulation()
width2, height2, rgb2, depth2, seg2 = pb.getCameraImage(
width=width,
height=height,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
X[k, :3, :, :] = rgb0[:, :, :3].transpose(2, 0, 1)
X[k, 3:, :, :] = rgb1[:, :, :3].transpose(2, 0, 1)
X_next[k, :3, :, :] = rgb1[:, :, :3].transpose(2, 0, 1)
X_next[k, 3:, :, :] = rgb2[:, :, :3].transpose(2, 0, 1)
U[k, :] = u0
k += 1
update_progress(float(k) / num_samples)
pb.disconnect()
return X, X_next, U
def create_new_world(self, furniture_type='wheelchair', static_human_base=False, human_impairment='random', print_joints=False, gender='random'):
p.resetSimulation(physicsClientId=self.id)
# Configure camera position
p.resetDebugVisualizerCamera(cameraDistance=1.75, cameraYaw=-25, cameraPitch=-45, cameraTargetPosition=[-0.2, 0, 0.4], physicsClientId=self.id)
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 0, physicsClientId=self.id)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, physicsClientId=self.id)
# Load all models off screen and then move them into place
p.loadURDF(os.path.join(self.directory, 'plane', 'plane.urdf'), physicsClientId=self.id)
# Disable rendering during creation
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0, physicsClientId=self.id)
if furniture_type == 'wheelchair':
# Create wheelchair
if self.robot_type in ['jaco', 'kinova_gen3']:
furniture = p.loadURDF(os.path.join(self.directory, 'wheelchair', 'wheelchair_jaco.urdf' if self.task not in ['dressing'] else 'wheelchair_jaco_left.urdf'), physicsClientId=self.id)
else:
furniture = p.loadURDF(os.path.join(self.directory, 'wheelchair', 'wheelchair.urdf'), physicsClientId=self.id)
# Initialize chair position
p.resetBasePositionAndOrientation(furniture, [0, 0, 0.06], p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
elif furniture_type == 'bed':
mesh_scale = [1.1]*3
bed_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced.obj'), rgbaColor=[1, 1, 1, 1], specularColor=[0.2, 0.2, 0.2], meshScale=mesh_scale, physicsClientId=self.id)
bed_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced_vhacd.obj'), meshScale=mesh_scale, flags=p.GEOM_FORCE_CONCAVE_TRIMESH, physicsClientId=self.id)
furniture = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=bed_collision, baseVisualShapeIndex=bed_visual, basePosition=[0, 0, 0], useMaximalCoordinates=True, physicsClientId=self.id)
# Initialize bed position
p.resetBasePositionAndOrientation(furniture, [-0.1, 0, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
elif furniture_type == 'table':
furniture = p.loadURDF(os.path.join(self.directory, 'table', 'table.urdf'), basePosition=[0, -0.35, 0], baseOrientation=p.getQuaternionFromEuler([0, 0, np.pi/2.0], physicsClientId=self.id), physicsClientId=self.id)
else:
furniture = None
# Choose gender
if gender not in ['male', 'female']:
gender = self.np_random.choice(['male', 'female'])
# Specify human impairments
if human_impairment == 'random':
human_impairment = self.np_random.choice(['none', 'limits', 'weakness', 'tremor'])
elif human_impairment == 'no_tremor':
human_impairment = self.np_random.choice(['none', 'limits', 'weakness'])
self.human_impairment = human_impairment
self.human_limit_scale = 1.0 if human_impairment != 'limits' else self.np_random.uniform(0.5, 1.0)
self.human_strength = 1.0 if human_impairment != 'weakness' else self.np_random.uniform(0.25, 1.0)
human, human_lower_limits, human_upper_limits = self.init_human(static_human_base, self.human_limit_scale, print_joints, gender=gender)
p.setTimeStep(self.time_step, physicsClientId=self.id)
# Disable real time simulation so that the simulation only advances when we call stepSimulation
p.setRealTimeSimulation(0, physicsClientId=self.id)
if self.robot_type == 'pr2':
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = self.init_pr2(print_joints)
elif self.robot_type == 'sawyer':
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = self.init_sawyer(print_joints)
elif self.robot_type == 'baxter':
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = self.init_baxter(print_joints)
elif self.robot_type == 'jaco':
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = self.init_jaco(print_joints)
elif self.robot_type == 'panda':
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = self.init_panda(print_joints)
elif self.robot_type == 'kinova_gen3':
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = self.init_kinova_gen3(print_joints)
else:
robot, robot_lower_limits, robot_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices = None, None, None, None, None
return human, furniture, robot, robot_lower_limits, robot_upper_limits, human_lower_limits, human_upper_limits, robot_right_arm_joint_indices, robot_left_arm_joint_indices, gender
def init_simulator():
global boxId, reset, low_energy_mode, high_performance_mode, terrain, p
robot_start_pos = [0, 0, 0.42]
p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetSimulation()
p.setTimeStep(1.0/freq)
p.setGravity(0, 0, -9.8)
reset = p.addUserDebugParameter("reset", 1, 0, 0)
low_energy_mode = p.addUserDebugParameter("low_energy_mode", 1, 0, 0)
high_performance_mode = p.addUserDebugParameter("high_performance_mode", 1, 0, 0)
p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
# p.setPhysicsEngineParameter(numSolverIterations=30)
# p.setPhysicsEngineParameter(enableConeFriction=0)
# planeId = p.loadURDF("plane.urdf")
# p.changeDynamics(planeId, -1, lateralFriction=1.0)
# boxId = p.loadURDF("/home/wgx/Workspace_Ros/src/cloudrobot/src/quadruped_robot.urdf", robot_start_pos,
# useFixedBase=FixedBase)
heightPerturbationRange = 0.06
numHeightfieldRows = 256
numHeightfieldColumns = 256
if terrain == "plane":
planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE)
ground_id = p.createMultiBody(0, planeShape)
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
elif terrain == "random1":
heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
for j in range(int(numHeightfieldColumns/2)):
for i in range(int(numHeightfieldRows/2)):
height = random.uniform(0, heightPerturbationRange)
heightfieldData[2*i+2*j*numHeightfieldRows] = height
heightfieldData[2*i+1+2*j*numHeightfieldRows] = height
heightfieldData[2*i+(2*j+1)*numHeightfieldRows] = height
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1], heightfieldTextureScaling=(
numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
ground_id = p.createMultiBody(0, terrainShape)
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
elif terrain == "random2":
terrain_shape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[.5, .5, .5],
fileName="heightmaps/ground0.txt",
heightfieldTextureScaling=128)
ground_id = p.createMultiBody(0, terrain_shape)
path = rospack.get_path('quadruped_ctrl')
textureId = p.loadTexture(path+"/model/grass.png")
p.changeVisualShape(ground_id, -1, textureUniqueId=textureId)
p.resetBasePositionAndOrientation(ground_id, [1, 0, 0.2], [0, 0, 0, 1])
elif terrain == "stairs":
planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE)
ground_id = p.createMultiBody(0, planeShape)
# p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0.0872, 0, 0.9962])
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
# many box
colSphereId = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.01])
colSphereId1 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02])
colSphereId2 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.03])
colSphereId3 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04])
# colSphereId4 = p.createCollisionShape(
# p.GEOM_BOX, halfExtents=[0.03, 0.03, 0.03])
p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
p.changeDynamics(colSphereId, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0])
p.changeDynamics(colSphereId1, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0])
p.changeDynamics(colSphereId2, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0])
p.changeDynamics(colSphereId3, -1, lateralFriction=lateralFriction)
# p.createMultiBody(10, colSphereId4, basePosition=[2.7, 1.0, 0.0])
# p.changeDynamics(colSphereId4, -1, lateralFriction=0.5)
p.changeDynamics(ground_id, -1, lateralFriction=lateralFriction)
boxId = p.loadURDF("mini_cheetah/mini_cheetah.urdf", robot_start_pos,
useFixedBase=False)
p.changeDynamics(boxId, 3, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 7, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 11, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 15, spinningFriction=spinningFriction)
jointIds = []
for j in range(p.getNumJoints(boxId)):
p.getJointInfo(boxId, j)
jointIds.append(j)
# slope terrain
# colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.5, 0.5, 0.001])
# colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.5, 0.06])
# BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.6, 1.0, 0.0])
# stairs
# colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02])
# colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04])
# colSphereId2 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.06])
# colSphereId3 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.08])
# colSphereId4 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1.0, 0.4, 0.1])
# BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId4, basePosition=[2.7, 1.0, 0.0])
reset_robot()
def __init__(self, show_pb, numsawyers):
if show_pb == 1:
pybullet.connect(pybullet.GUI)
else:
pybullet.connect(pybullet.DIRECT)
pybullet.resetSimulation()
import pybullet_data
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
self.plane = pybullet.loadURDF("plane.urdf", basePosition=[0, 0, -1])
pybullet.setTimeStep(0.0001)
self.cabinet = pybullet.loadURDF(
'/home/rachel/rawhide/rmh_code/cylinder_wire.urdf',
basePosition=[.7, 0, 0],
useFixedBase=1)
if numsawyers == 2:
self.sawyer1 = pybullet.loadURDF(
'/home/rachel/rawhide/rmh_code/pybullet_robots/data/sawyer_robot/sawyer_description/urdf/sawyer_rmh.urdf',
basePosition=[0, -.35, 0],
useFixedBase=1)
self.sawyer2 = pybullet.loadURDF(
'/home/rachel/rawhide/rmh_code/pybullet_robots/data/sawyer_robot/sawyer_description/urdf/sawyer_rmh.urdf',
basePosition=[0, .35, 0],
useFixedBase=1)
self.sawyer_joint_indexes = [3, 8, 9, 10, 11, 13, 16]
p1pos = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
self.zeropos = [0, 0, 0, 0, 0, 0, 0]
self.neutralpos = [0.00, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161]
for j in range(len(p1pos)):
pybullet.resetJointState(self.sawyer1,
self.sawyer_joint_indexes[j],
p1pos[j])
pybullet.resetJointState(self.sawyer2,
self.sawyer_joint_indexes[j],
p1pos[j])
time.sleep(5)
for j in range(len(self.zeropos)):
pybullet.resetJointState(self.sawyer1,
self.sawyer_joint_indexes[j],
self.zeropos[j])
pybullet.resetJointState(self.sawyer2,
self.sawyer_joint_indexes[j],
self.zeropos[j])
else:
self.sawyer1 = pybullet.loadURDF(
'/home/rachel/rawhide/rmh_code/pybullet_robots/data/sawyer_robot/sawyer_description/urdf/sawyer_rmh.urdf',
basePosition=[0, -.35, 0],
useFixedBase=1)
self.sawyer_joint_indexes = [3, 8, 9, 10, 11, 13, 16]
p1pos = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
self.zeropos = [0, 0, 0, 0, 0, 0, 0]
self.neutralpos = [0.00, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161]
for j in range(len(p1pos)):
pybullet.resetJointState(self.sawyer1,
self.sawyer_joint_indexes[j],
p1pos[j])
time.sleep(5)
for j in range(len(self.zeropos)):
pybullet.resetJointState(self.sawyer1,
self.sawyer_joint_indexes[j],
self.zeropos[j])
import pybullet as p
import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
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], [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],
[-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], [1.000000, -1.000000, 1.000000],
def setup(self):
"""Sets up the robot + tray + objects.
"""
test = self._test
if not self._urdf_list: # Load from procedural random objects.
if not self._object_filenames:
self._object_filenames = self._get_random_objects(
num_objects=self._num_objects,
test=test,
replace=self._allow_duplicate_objects,
)
self._urdf_list = self._object_filenames
logging.info('urdf_list %s', self._urdf_list)
pybullet.resetSimulation(physicsClientId=self.cid)
pybullet.setPhysicsEngineParameter(numSolverIterations=150,
physicsClientId=self.cid)
pybullet.setTimeStep(self._time_step, physicsClientId=self.cid)
pybullet.setGravity(0, 0, -10, physicsClientId=self.cid)
plane_path = os.path.join(self._urdf_root, 'plane.urdf')
pybullet.loadURDF(plane_path, [0, 0, -1], physicsClientId=self.cid)
table_path = os.path.join(self._urdf_root, 'table/table.urdf')
pybullet.loadURDF(table_path, [0.5, 0.0, -.82], [0., 0., 0., 1.],
physicsClientId=self.cid)
self._kuka = kuka.Kuka(urdfRootPath=self._urdf_root,
timeStep=self._time_step,
clientId=self.cid)
self._block_uids = []
self._urdf_list = [
('/home/jonathan/Desktop/Projects/objects/6a0c8c43b13693fa46eb89c2e933753d.obj',
'obj', [4, 4, 4]),
('/home/jonathan/Desktop/Projects/objects/6aec84952a5ffcf33f60d03e1cb068dc.obj',
'obj', [2, 2, 2]),
('/home/jonathan/Desktop/Projects/objects/bed29baf625ce9145b68309557f3a78c.obj',
'obj', [2, 2, 2]),
('/home/jonathan/Desktop/Projects/objects/dac6ea4929f1e47f178d703a993fe24c.obj',
'obj', [2, 2, 2]),
('/home/jonathan/Desktop/Projects/objects/f452c1053f88cd2fc21f7907838a35d1.obj',
'obj', [2, 2, 2])
]
for name in self._urdf_list:
if (name[1] == 'urdf'):
print(name[0], name[1])
urdf_name = name[0]
xpos = 0.4 + self._block_random * random.random()
ypos = self._block_random * (random.random() - .5)
angle = np.pi / 2 + self._block_random * np.pi * random.random(
)
ori = pybullet.getQuaternionFromEuler([0, 0, angle])
uid = pybullet.loadURDF(urdf_name, [xpos, ypos, .15],
[ori[0], ori[1], ori[2], ori[3]],
physicsClientId=self.cid)
self._block_uids.append(uid)
for _ in range(500):
pybullet.stepSimulation(physicsClientId=self.cid)
if (name[1] == 'obj'):
obj_name = name[0]
scale = name[2]
xpos = 0.4 + self._block_random * random.random()
ypos = self._block_random * (random.random() - .5)
angle = np.pi / 2 + self._block_random * np.pi * random.random(
)
ori = pybullet.getQuaternionFromEuler([0, 0, angle])
uid = pybullet.createMultiBody(
0.05,
pybullet.createCollisionShape(pybullet.GEOM_MESH,
fileName=name[0],
meshScale=scale),
pybullet.createVisualShape(pybullet.GEOM_MESH,
fileName=name[0],
meshScale=scale),
[xpos, ypos, .15], [ori[0], ori[1], ori[2], ori[3]],
physicsClientId=self.cid)
self._block_uids.append(uid)
for _ in range(500):
pybullet.stepSimulation(physicsClientId=self.cid)
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=[]
# print("active_joints_info::",active_joints_info)
# print("finger::",finger)
# print("`num of joints:::",num_joints)
"""
for i in range(num_joints):
j_info = p.getJointInfo(fingerID,i)
print("joint_info::",j_info)
"""
# texUid = p.loadTexture("./../cube_new/aaa.png")
# cube_objects = p.loadSDF("./../cube_new/model.sdf")
# p.changeVisualShape(cube_objects[0], -1, rgbaColor=[1, 1, 1, 1])
# p.changeVisualShape(cube_objects[0], -1, textureUniqueId=texUid)
# p.resetBasePositionAndOrientation(cube_objects[0], [0, 0.37, 0.07],[0.7071, 0.000000, 0.000000, 0.7071])
p.setRealTimeSimulation(0)
p.setTimeStep(1. / 5000)
while (1):
p.resetBasePositionAndOrientation(fingerID, [0, 0, 0],
[0.7071, 0.000000, 0.000000, -0.7071])
parent_list = []
for i in range(num_active_joints):
jointIndex = active_joints_info[i]["jointIndex"]
jointName = active_joints_info[i]["jointName"]
linkName = active_joints_info[i]["linkName"]
jointPositionState = p.getJointState(fingerID, jointIndex)[0]
# print("linkName::",linkName)
# print("jointName::",jointName)
# print("jointIndex::",jointIndex)
# print("jointPositionState::",jointPositionState)
jointll = active_joints_info[i]["jointLowerLimit"]
jointul = active_joints_info[i]["jointUpperLimit"]