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 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 disconnect(self):
p.disconnect()
def test_loadurdf(self):
import pybullet as p
p.connect(p.DIRECT)
ob = p.loadURDF("r2d2.urdf")
self.assertEqual(ob, 0)
p.disconnect()
tiltAngles.append(np.arctan2(upDir[1], upDir[0]))
print("next")
print(data[0][0][8:len(data[0][0])])
print(predictedJointState)
inState = torch.FloatTensor(tiltAngles + predictedTwist +
predictedJointState).unsqueeze(0).to(device)
#inState = torch.FloatTensor(tiltAngles + predictedTwist + data[0][0][8:len(data[0][0])]).unsqueeze(0).to(device)
#inState = torch.FloatTensor(data[0][0]).unsqueeze(0).to(device)
inAction = torch.FloatTensor(data[0][2]).unsqueeze(0).to(device)
inMap = torch.from_numpy(data[0][1]).unsqueeze(0).float().to(device)
prediction = motionModel([inState, inMap, inAction])[0]
relativePos = prediction[0, 0:3]
relativeOrien = prediction[0, 3:7]
predictedTwist = prediction[0, 7:13].tolist()
predictedJointState = prediction[0, 13:27].tolist()
predictedPose = p.multiplyTransforms(predictedPose[0], predictedPose[1],
relativePos, relativeOrien)
actualX.append(data[3][0][0])
actualY.append(data[3][0][1])
predX.append(predictedPose[0][0])
predY.append(predictedPose[0][1])
p.disconnect(physicsClientId=physicsClientId)
plt.figure()
for i in range(len(actualX)):
plt.clf()
plt.plot(actualX[0:i], actualY[0:i])
plt.plot(predX[0:i], predY[0:i])
plt.xlim((-10, 10))
plt.ylim((-10, 10))
plt.pause(0.1)
plt.show()
import pybullet as p
cin = p.connect(p.SHARED_MEMORY)
if (cin < 0):
cin = p.connect(p.GUI)
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)]
objects = p.loadMJCF("MPL/mpl2.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
cid0 = p.createConstraint(10,-1,-1,-1,p.JOINT_FIXED,[0.000000,0.000000,0.000000],[-0.050000,0.000000,0.020000],[0.500000,0.300006,0.700000],[0.707388,-0.000563,0.706825,0.000563],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid0,maxForce=500.000000)
p.setGravity(0.000000,0.000000,-10.000000)
p.stepSimulation()
p.disconnect()
"""Calculate the scalar Euclidean distance between two objects."""
pos1 = p.getBasePositionAndOrientation(obj1)[0]
pos2 = p.getBasePositionAndOrientation(obj2)[0]
return np.sqrt(np.sum(np.subtract(pos1, pos2)**2))
def up_force(obj, mag):
"""Upward force on obj of a given magnitude mag."""
p.applyExternalForce(objectUniqueId=obj,
linkIndex=-1,
forceObj=[0, 0, mag],
posObj=[0, 0, 0],
flags=p.LINK_FRAME)
def sim(n_steps, animate=True):
for i in range(n_steps):
up_force(globals.sph2, 9)
p.stepSimulation()
if animate:
time.sleep(globals.t_step)
globals.curr_step += n_steps
if __name__ == "__main__":
init_sim()
sim(500)
time.sleep(15)
p.disconnect(globals.physicsCilent)
def __del__(self):
"""Clean up connection if not already done."""
try:
pybullet.disconnect(physicsClientId=self._client)
except pybullet.error:
pass
def run(self, theta):
# load arguments
block_mass, = theta
# connect and configure sim
if self.debug:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.setRealTimeSimulation(False)
p.setGravity(0, 0, -10)
# load arm
model_path = "jaco_robotiq_object.urdf"
arm_id = p.loadURDF(model_path, [0, 0, 0], useFixedBase=True)
# variables specific to this arm
eef_index = 8
num_joints = 18
rp = [
-pi / 2, pi * 5 / 4, 0., pi / 2, 0., pi * 5 / 4, -pi / 2, 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0.
]
ll = [-pi] * num_joints
ul = [pi] * num_joints
jd = [0.000001] * num_joints
jr = np.array(ll) - np.array(ul)
ik_solver = p.IK_DLS
# configure arm
for i in range(num_joints):
p.resetJointState(arm_id, i, rp[i])
p.enableJointForceTorqueSensor(arm_id, i)
# extract joint names
joint_id = {}
for i in range(p.getNumJoints(arm_id)):
jointInfo = p.getJointInfo(arm_id, i)
joint_id[jointInfo[1].decode('UTF-8')] = jointInfo[0]
# adjust block mass
weight_joint_id = joint_id["weight_joint"]
p.changeDynamics(arm_id, weight_joint_id, mass=block_mass)
# set up variables needed during sim
len_seconds = 5
# number of steps to wait before recording torques
burn_in = 150
start_time = time.time()
controlled_joints = list(range(eef_index - 1))
num_controlled_joints = eef_index - 1
orn = p.getQuaternionFromEuler([math.pi / 2, math.pi / 2, math.pi / 2])
targetVelocities = [0] * num_controlled_joints
forces = [500] * num_controlled_joints
positionGains = [0.03] * num_controlled_joints
velocityGains = [1] * num_controlled_joints
# time counter
t = 0.
# store torques as we go
torques = []
for steps in range(240 * len_seconds + burn_in):
p.stepSimulation()
t += 0.005
if self.debug:
time.sleep(1 / 240)
# move the arm
pos = [0.2 * math.cos(t), -0.3, 0.3 + 0.2 * math.sin(t)]
jointPoses = p.calculateInverseKinematics(
arm_id,
eef_index,
pos,
orn,
lowerLimits=ll,
upperLimits=ul,
jointRanges=jr,
restPoses=rp,
jointDamping=jd,
solver=ik_solver)[:num_controlled_joints]
p.setJointMotorControlArray(bodyIndex=arm_id,
jointIndices=controlled_joints,
controlMode=p.POSITION_CONTROL,
targetPositions=jointPoses,
targetVelocities=targetVelocities,
forces=forces,
positionGains=positionGains,
velocityGains=velocityGains)
# get the torque applied at each joint as reported by the force/torque sensors
if steps > burn_in:
js = p.getJointStates(arm_id, controlled_joints)
torques.append([j[3] for j in js])
# plotting
# fig, ax = plt.subplots()
# ax.plot(torques)
# ax.set_ylim([-20, 20])
# plt.savefig("mass_{}.png".format(block_mass))
p.disconnect()
if np.any(np.isnan(torques)):
print("torques contains {} NaN values!".format(
np.sum(np.isnan(torques))))
return np.array(torques).transpose()
def run_simulation(hand_verts,
hand_faces,
obj_verts,
obj_faces,
conn_id=None,
vhacd_exe=None,
sample_idx=None,
save_video=False,
save_video_path=None,
simulation_step=1 / 240,
num_iterations=35,
object_friction=3,
hand_friction=3,
hand_restitution=0,
object_restitution=0.5,
object_mass=1,
verbose=False,
vhacd_resolution=1000,
wait_time=0,
save_hand_path=None,
save_obj_path=None,
save_simul_folder=None,
use_gui=False):
if conn_id is None:
if use_gui:
conn_id = p.connect(p.GUI)
else:
conn_id = p.connect(p.DIRECT)
hand_indicies = hand_faces.flatten().tolist()
p.resetSimulation(physicsClientId=conn_id)
p.setPhysicsEngineParameter(enableFileCaching=0, physicsClientId=conn_id)
p.setPhysicsEngineParameter(numSolverIterations=150,
physicsClientId=conn_id)
p.setPhysicsEngineParameter(fixedTimeStep=simulation_step,
physicsClientId=conn_id)
p.setGravity(0, 9.8, 0, physicsClientId=conn_id)
# add hand
base_tmp_dir = "tmp/objs"
os.makedirs(base_tmp_dir, exist_ok=True)
hand_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir)
save_obj(hand_tmp_fname, hand_verts, hand_faces)
if save_hand_path is not None:
shutil.copy(hand_tmp_fname, save_hand_path)
hand_collision_id = p.createCollisionShape(
p.GEOM_MESH,
fileName=hand_tmp_fname,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH,
indices=hand_indicies,
physicsClientId=conn_id)
hand_visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=hand_tmp_fname,
rgbaColor=[0, 0, 1, 1],
specularColor=[0, 0, 1],
physicsClientId=conn_id)
hand_body_id = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=hand_collision_id,
baseVisualShapeIndex=hand_visual_id,
physicsClientId=conn_id)
p.changeDynamics(hand_body_id,
-1,
lateralFriction=hand_friction,
restitution=hand_restitution,
physicsClientId=conn_id)
obj_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir)
os.makedirs(base_tmp_dir, exist_ok=True)
# Save object obj
if save_obj_path is not None:
final_obj_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir)
save_obj(final_obj_tmp_fname, obj_verts, obj_faces)
shutil.copy(final_obj_tmp_fname, save_obj_path)
# Get obj center of mass
obj_center_mass = np.mean(obj_verts, axis=0)
obj_verts -= obj_center_mass
# add object
use_vhacd = True
if use_vhacd:
if verbose:
print("Computing vhacd decomposition")
time1 = time.time()
# convex hull decomposition
save_obj(obj_tmp_fname, obj_verts, obj_faces)
if not vhacd(obj_tmp_fname, vhacd_exe, resolution=vhacd_resolution):
raise RuntimeError("Cannot compute convex hull "
"decomposition for {}".format(obj_tmp_fname))
else:
print(f"Succeeded vhacd decomp of {obj_tmp_fname}")
obj_collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_tmp_fname,
physicsClientId=conn_id)
if verbose:
time2 = time.time()
print("Computed v-hacd decomposition at res {} {:.6f} s".format(
vhacd_resolution, (time2 - time1)))
else:
obj_collision_id = p.createCollisionShape(p.GEOM_MESH,
vertices=obj_verts,
physicsClientId=conn_id)
obj_visual_id = p.createVisualShape(
p.GEOM_MESH,
fileName=obj_tmp_fname,
rgbaColor=[1, 0, 0, 1],
specularColor=[1, 0, 0],
physicsClientId=conn_id,
)
obj_body_id = p.createMultiBody(
baseMass=object_mass,
basePosition=obj_center_mass,
baseCollisionShapeIndex=obj_collision_id,
baseVisualShapeIndex=obj_visual_id,
physicsClientId=conn_id,
)
p.changeDynamics(
obj_body_id,
-1,
lateralFriction=object_friction,
restitution=object_restitution,
physicsClientId=conn_id,
)
# simulate for several steps
if save_video:
images = []
if use_gui:
renderer = p.ER_BULLET_HARDWARE_OPENGL
else:
renderer = p.ER_TINY_RENDERER
save_video_path = "simulate_video" if save_video_path is None else save_video_path
os.makedirs(save_video_path, exist_ok=True)
sample_idx = 0 if sample_idx is None else sample_idx
save_video_path = os.path.join(save_video_path,
"{:08d}.gif".format(sample_idx))
for step_idx in range(num_iterations):
p.stepSimulation(physicsClientId=conn_id)
if save_video:
img = take_picture(renderer, conn_id=conn_id)
images.append(img)
if save_simul_folder:
hand_step_path = os.path.join(save_simul_folder,
"{:08d}_hand.obj".format(step_idx))
shutil.copy(hand_tmp_fname, hand_step_path)
obj_step_path = os.path.join(save_simul_folder,
"{:08d}_obj.obj".format(step_idx))
pos, orn = p.getBasePositionAndOrientation(obj_body_id,
physicsClientId=conn_id)
mat = np.reshape(p.getMatrixFromQuaternion(orn), (3, 3))
obj_verts_t = pos + np.dot(mat, obj_verts.T).T
save_obj(obj_step_path, obj_verts_t, obj_faces)
time.sleep(wait_time)
if save_video:
write_video(images, save_video_path)
print("Saved gif to {}".format(save_video_path))
pos_end = p.getBasePositionAndOrientation(obj_body_id,
physicsClientId=conn_id)[0]
if use_vhacd:
os.remove(obj_tmp_fname)
if save_obj_path is not None:
os.remove(final_obj_tmp_fname)
os.remove(hand_tmp_fname)
distance = np.linalg.norm(pos_end - obj_center_mass)
p.disconnect(physicsClientId=conn_id)
return distance
costs_precomputed_for_est = precompute_environment_costs(numEnvs, K, L, params, husky, sphere, GUI, seed)
print("Done precomputing.")
print("Estimating true cost for PAC-Bayes controller based on ", numEnvs, " environments...")
true_cost_pac = expected_cost(p_opt_pac, costs_precomputed_for_est)
print("Done estimating true cost.")
################################################################################
# Print results
print("Time for precomputation: ", time_precomputation)
print("Time for optimization: ", time_optimization)
print("Cost bound (PAC-Bayes): ", pac_bound)
print("True cost estimate (PAC-Bayes): ", true_cost_pac)
# Disconect from pybullet
pybullet.disconnect()
################################################################################
# Run optimized controller on some environments to visualize
# Flag that sets if things are visualized
GUI = True;
pybullet.connect(pybullet.GUI)
# Clean up probabilities
p_opt_pac = np.maximum(p_opt_pac, 0)
p_opt_pac = p_opt_pac/np.sum(p_opt_pac)
random_seed = 25 #
numEnvs = 10 # Number of environments to show videos for
def dispose(self):
p.disconnect()
time.sleep(5)
def destroy(self):
p.disconnect(self.physicsClient)
def disconnect(self):
if self.is_active:
pb.disconnect(physicsClientId=self.server_id)
self.bodies = {}
self.is_active = False
def _close(self): if (self.ownsPhysicsClient): if (self.physicsClientId>=0): p.disconnect(self.physicsClientId) self.physicsClientId = -1
def kill(self):
p.disconnect()
def __exit__(self,*_,**__):
pybullet.disconnect()
def deactivate_viewer(self):
p.disconnect()
def __del__(self): p.disconnect()
def signal_handler(signal, frame):
if SimConfig.VIDEO_RECORD:
pybullet_util.make_video(video_dir)
p.disconnect()
sys.exit(0)
def __exit__(self,*_,**__):
pybullet.disconnect()
def close(self):
"""Close environment.
"""
p.disconnect(self.physicsClient)
def close(self):
p.disconnect()
def __del__(self):
if CONNECTED_TO_SIMULATOR:
p.disconnect()
def test_connect_direct(self): import pybullet as p cid = p.connect(p.DIRECT) self.assertEqual(cid, 0) p.disconnect()
def shutdown(self):
p.disconnect()
def grasp(self,
zs,
objPos,
objOrn,
objPath,
gui,
save_figure=False,
figure_path=None,
mu=None,
sigma=None):
# Connect to an PB instance
if gui:
p.connect(p.GUI, options="--width=2600 --height=1800")
p.resetDebugVisualizerCamera(0.8, 180, -45, [0.5, 0, 0])
else:
p.connect(p.DIRECT)
zs = zs.reshape(1, -1)
######################### Reset #######################
self.reset()
# Load object
if len(objOrn) == 3: # input Euler angles
objOrn = p.getQuaternionFromEuler(objOrn)
self._objId = p.loadURDF(objPath,
basePosition=objPos,
baseOrientation=objOrn)
p.changeDynamics(self._objId,
-1,
lateralFriction=self.env._mu,
spinningFriction=self.env._sigma,
frictionAnchor=1,
mass=0.1)
# Let the object settle
for _ in range(20):
p.stepSimulation()
######################### Decision #######################
initial_ee_pos_before_depth = array([0.3, -0.5, 0.35])
initial_ee_orn = array([1.0, 0.0, 0.0, 0.0])
# Set arm to a pose away from camera image, keep fingers open
self.env.reset_arm_joints_ik(initial_ee_pos_before_depth,
initial_ee_orn,
fingerPos=0.04)
# Get observations
depth = ((0.7 - self.get_depth()) / 0.20).clip(max=1.0)
if gui:
plt.imshow(depth, cmap='Greys', interpolation='nearest')
plt.show()
depth = torch.from_numpy(depth).float().unsqueeze(0).unsqueeze(0)
# Infer action
pred = self.actor(depth, zs).squeeze(0).detach().numpy()
target_pos = pred[:3]
target_pos[:2] /= 20
target_pos[0] += 0.5 # add offset
target_yaw = np.arctan2(pred[3], pred[4])
if target_yaw < -np.pi / 2:
target_yaw += np.pi
elif target_yaw > np.pi / 2:
target_yaw -= np.pi
######################## Motion ##########################
# Reset to target pos above
target_pos_above = target_pos + array([0., 0., 0.05])
target_euler = [-1.57, 3.14, 1.57 - target_yaw]
for _ in range(3):
self.env.reset_arm_joints_ik(target_pos_above,
euler2quat(target_euler),
fingerPos=0.04)
self.env.grasp(targetVel=0.10) # keep finger open before grasping
# Reach down to target pos, check if hits the object
_, success = self.env.move_pos(
absolute_pos=target_pos,
absolute_global_euler=target_euler,
numSteps=150,
# checkContact=self.checkContact,
checkPalmContact=self.checkPalmContact,
objId=self._objId)
if not success:
p.disconnect()
return success
# Grasp
self.env.grasp(targetVel=-0.10)
self.env.move_pos(absolute_pos=target_pos,
absolute_global_euler=target_euler,
numSteps=100)
# Lift
self.env.move_pos(absolute_pos=target_pos_above,
absolute_global_euler=target_euler,
numSteps=150)
# Check success
table_mug_contact = p.getContactPoints(self._objId,
self.env._tableId,
linkIndexA=-1,
linkIndexB=-1)
table_mug_contact = [i for i in table_mug_contact if i[9] > 1e-3]
if len(table_mug_contact) == 0:
success = 1
else:
success = 0
# Close instance and return result
p.disconnect()
return success
def check_stability(input_file, gui=False):
# First, check if the file is even rooted.
# If it's not rooted, it can't be stable
if not check_rooted(input_file):
return False
# Start up the simulation
mode = p.GUI if gui else p.DIRECT
physicsClient = p.connect(mode)
p.setGravity(0, 0, -9.8)
# Load the ground plane
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# print(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
# Convert the object to a URDF assembly, load it up
# There may be more than one URDF, if the object had more than one connected component
obj2urdf(input_file, 'tmp')
objIds = []
startPositions = {}
scales = {}
for urdf in [
f for f in os.listdir('tmp') if os.path.splitext(f)[1] == '.urdf'
]:
with open(f'tmp/{os.path.splitext(urdf)[0]}.json', 'r') as f:
data = json.loads(f.read())
startPos = data['start_pos']
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
objId = p.loadURDF(f"tmp/{urdf}", startPos, startOrientation)
objIds.append(objId)
startPositions[objId] = startPos
scales[objId] = data['scale']
shutil.rmtree('tmp')
# Disable collisions between all objects (we only want collisions between objects and the ground)
# That's because we want to check if the different components are *independently* stable, and
# having them hit each other might muck up that judgment
for i in range(0, len(objIds) - 1):
ni = p.getNumJoints(objIds[i])
for j in range(i + 1, len(objIds)):
nj = p.getNumJoints(objIds[j])
for k in range(-1, ni):
for l in range(-1, nj):
p.setCollisionFilterPair(objIds[i], objIds[j], k, l, False)
# See if objects are stable under some perturbation
for objId in objIds:
s = scales[objId]
p.applyExternalForce(objId, -1, (0, 0, 600 * s), startPositions[objId],
p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (0, 5 * s, 0), p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (5 * s, 0, 0), p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (0, 0, 200 * s), p.WORLD_FRAME)
# Run simulation
if gui:
while True:
p.stepSimulation()
time.sleep(1. / 240.)
else:
for i in range(10000):
p.stepSimulation()
for objId in objIds:
endPos, _ = p.getBasePositionAndOrientation(objId)
zend = endPos[2]
zstart = startPositions[objId][2]
zdiff = abs(zend - zstart)
if zdiff > abs(0.05 * scales[objId]):
p.disconnect()
return False
p.disconnect()
return True
def get_query(node, subscriber, interactive=True):
"""Creates a simulation of the arrangement detected by the robot and uses it
to find an optimal arrangement of more bodies."""
# Create node
rospy.init_node(node)
# Display project info
pkg_path = rospkg.RosPack().get_path('experiment')
eb.display_image(pkg_path + '/share/images/main.png')
# Define camera poses
org_surface_pose = [0.65, 0.4, 0.15, np.pi, 0.0, 0.0]
new_surface_pose = [0.25, 0.85, 0.15, np.pi, 0.0, 0.0]
# Initialize robot and move left arm to initial pose.
eb.enable_robot()
limb = eb.Arm("left")
limb.move_to_pose(org_surface_pose)
eb.display_image(pkg_path + '/share/images/step1.png')
# Begin constructing query dic
s_name = "surface"
s_path = URDF_DIR + "{}.urdf".format(s_name)
s_info = {
"path": s_path,
"pose": [0.0, 0.0, 0.0],
"z offset": 0.05,
"area": 0.0855
}
overall_config = {s_name: s_info, "obstacles": {}}
if interactive:
# Initialize physics engine and environment.
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(3, 0, -89.99, [0, 0, 0])
# Load surface to physics engine
s_id = p.loadURDF(s_path, useFixedBase=True, globalScaling=10)
p.changeVisualShape(s_id, -1, rgbaColor=[1, 0.992, 0.816, 1])
# Get readings from camera
arrangement = obtain_arrangement(subscriber)
# Get body ids from physics engine and the dictionary of the original objs
loaded_body_ids, config_dic = extract_query(arrangement, interactive)
rate = rospy.Rate(10)
org_flag = False
new_flag = False
moved = False
while not rospy.is_shutdown():
char = eb.get_char(5)
if char in ['\x1b', '\x03']:
p.disconnect()
rospy.signal_shutdown("Shutting down.")
# Signal satisfaction with detected org config
if char == 'o':
org_flag = True
# Signal satisfaction with detected new config
if char == 'n':
new_flag = True
# Get readings of org config from camera again
if not org_flag:
arrangement = obtain_arrangement(subscriber)
for i in loaded_body_ids:
p.removeBody(i)
loaded_body_ids, config_dic = extract_query(arrangement,
interactive=True)
# Get readings of new config from camera (again)
elif org_flag and not new_flag:
if not moved:
limb.move_to_pose(new_surface_pose)
moved = True
overall_config.update({"originals": config_dic})
arrangement = obtain_arrangement(subscriber)
for i in loaded_body_ids:
p.removeBody(i)
loaded_body_ids, config_dic = extract_query(arrangement,
interactive=True,
new=True)
elif org_flag and new_flag:
# dump the json
overall_config.update({"news": config_dic})
# shutdown and exit
p.disconnect()
my_query = json.dumps(overall_config)
placement = generate_placement(my_query,
algo='middle',
cam_dist=3,
verbose=False)
ans_json = placement.dump_json()
with open("my_ans.json", "wb") as f:
json.dump(ans_json, f)
rospy.signal_shutdown("Shutting down.")
rate.sleep()
else:
# Get configuration of the main surface
rospy.sleep(5)
arrangement = obtain_arrangement(subscriber)
_, overall_config = extract_query(arrangement,
overall_config,
interactive=False)
# Get configuration of the surface with new objects
limb.move_to_pose(new_surface_pose)
rospy.sleep(5)
arrangement = obtain_arrangement(subscriber)
_, overall_config = extract_query(arrangement,
overall_config,
interactive=False,
new=True)
# Create the query from the configurations detected and save it
query_json = json.dumps(overall_config, sort_keys=True, indent=4)
with open("my_query.json", "wb") as f:
f.write(query_json)
# Find a solution for the placement problem and save it
eb.display_image(pkg_path + '/share/images/step2.png')
placement = generate_placement(json.loads(query_json))
ans_json = placement.dump_json()
with open("my_placement.json", "wb") as f:
f.write(ans_json)
# Find a solution for the planning problem and save it
eb.display_image(pkg_path + '/share/images/step3.png')
plan = generate_plan(placement, new=False)
plan.to_csv("my_plan.csv")
# Execute the rearrangement process
eb.display_image(pkg_path + '/share/images/step4.png')
gripper_height = 0.172
x_offset = 0.055
y_offset = -0.005
theta_offset = np.pi / 2
for index, step in plan.iterrows():
pick_x = (step["poseFrom"][1] /
10.0) + new_surface_pose[0] + x_offset
pick_y = (step["poseFrom"][0] /
10.0) + new_surface_pose[1] + y_offset
pick_theta = normalize(step["poseFrom"][2] + theta_offset)
pick_pose = [
pick_x, pick_y, new_surface_pose[2] - gripper_height, np.pi,
0.0, pick_theta
]
place_x = (step["poseTo"][1] /
10.0) + org_surface_pose[0] + x_offset
place_y = (step["poseTo"][0] /
10.0) + org_surface_pose[1] + y_offset
place_theta = normalize(step["poseTo"][2] + theta_offset)
place_pose = [
place_x, place_y, org_surface_pose[2] - gripper_height + 0.01,
np.pi, 0.0, place_theta
]
limb.pick_and_place(pick_pose, place_pose)
# Go back to initial image and exit
eb.display_image(pkg_path + '/share/images/main.png')
rospy.signal_shutdown("Shutting down.")
def computedTorqueControl(self,
th_initial,
th_final,
final_time=2,
controller_gain=400):
p.setRealTimeSimulation(False)
# get the desired trajectory
q_d, dq_d, ddq_d = self.getTrajectory(th_initial,
th_final,
tf=final_time,
dt=self.time_step)
traj_points = q_d.shape[0]
print('#Trajectory points:', traj_points)
# forward dynamics simulation loop
# for turning off link and joint damping
for link_idx in range(self.num_joints + 1):
p.changeDynamics(self.robot_id,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=0.0)
p.changeDynamics(self.robot_id, link_idx, maxJointVelocity=200)
# Enable torque control
p.setJointMotorControlArray(self.robot_id,
self.controllable_joints,
p.VELOCITY_CONTROL,
forces=np.zeros(
len(self.controllable_joints)))
Kp = controller_gain
Kd = 2 * np.sqrt(Kp)
n = 0
while n < q_d.shape[0]:
# get current joint states
q, dq, _ = self.getJointStates()
# PD control
q_e = q_d[n] - np.asarray(q)
dq_e = dq_d[n] - np.asarray(dq)
aq = ddq_d[n] + Kp * q_e + Kd * dq_e
tau = p.calculateInverseDynamics(self.robot_id, list(q), list(dq),
list(aq))
# tau += kd * dq_d[n] # if joint damping is turned off, this torque will not be required
# print(tau)
# torque control
p.setJointMotorControlArray(self.robot_id,
self.controllable_joints,
controlMode=p.TORQUE_CONTROL,
forces=tau)
print('n:{}::th:{}'.format(n, q))
p.stepSimulation()
time.sleep(self.time_step)
n += 1
print('Desired joint angles:', th_final)
p.disconnect()
if __name__ == '__main__':
env = PoseEnv()
agent = RandomAgent(env.robot.action_space)
goal = np.array([0.5, 0.5, 0.5])
done = False
flag = True
num = 10000
try:
while True:
action = {}
# action['arm'] = [100, 100, 100, 100, 100, 100]
# action['arm'] = [250, 0, 0, 0, 0, 0]
# action['arm'] = [0, 100, 0, 0, 0, 0]
# action['arm'] = [0, 0, 0, 500, 0, 0]
# action['arm'] = [0, 0, 0, 0, 400, 0]
# action['arm'] = [pi/4, 0, 0, 0, 0, 0]
# action['arm'] = [0, 0, pi/4, 0, 0, pi/3]
action = np.array([0, 0, pi / 4, 0, 0, pi / 3])
next_state, reward, done, _ = env.step(action, goal)
num -= 1
if num == 0:
num = 10000
env.reset()
# pdb.set_trace()
bullet.disconnect()
except:
bullet.disconnect()
def impedenceController(self,
th_initial,
desired_pose,
controller_gain=100):
p.setRealTimeSimulation(False)
# forward dynamics simulation loop
# for turning off link and joint damping
for link_idx in range(self.num_joints + 1):
p.changeDynamics(self.robot_id,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=0.0)
p.changeDynamics(self.robot_id, link_idx, maxJointVelocity=200)
# Enable torque control
p.setJointMotorControlArray(self.robot_id,
self.controllable_joints,
p.VELOCITY_CONTROL,
forces=np.zeros(
len(self.controllable_joints)))
kd = 0.7 # from URDF file
Kp = controller_gain
Kd = 2 * np.sqrt(Kp)
Md = 0.01 * np.eye(6)
# Target position and velcoity
# xd = np.array([0.4499998573193256, 0.1, 1.95035834701983, 0.0, 1.5707963267948966, 0.0]) #1-link
# xd = np.array([1.3499995719791142, 0.2, 2.9510750145148816, 0.0, 1.5707963267948966, 0.0]) #2-link
# xd = np.array([2.199999302511422, 0.3, 2.9517518416742643, 0.0, 1.5707963267948966, 0.0]) #3-link
# xd = np.array([1.8512362079506117, 0.30000000000000004, 4.138665008474901, -0.0, 1.0000000496605894, -0.0]) #3-link
# xd = np.array([0.10972055742719365, -0.716441307051838, 1.44670878280948, -1.5700006464761673, 0.0007970376813496536, -1.570796326772595]) #ur5-link
# xd = np.array([0.6811421738723965, -0.24773390188802563, 1.44670878280948, -1.5700006464761678, 0.0007970376813495148, -0.5007963267725951]) #ur5-link
# xd = np.array([-0.10857937593446423, 0.7166151451748437, 1.4467087828094798, -1.5700006464761673, 0.0007970376813502642, 1.5692036732274044]) #ur5-link
xd = desired_pose
dxd = np.zeros(len(self.controllable_joints))
# define GUI sliders
xdGUIids = self.TaskSpaceGUIcontrol(goal=xd)
ForceInitial = np.zeros(len(self.controllable_joints))
ForceGUIids = self.ForceGUIcontrol(forces=ForceInitial,
max_limit=10,
min_limit=-10)
while True:
# read GUI values
xd = self.readGUIparams(xdGUIids) # task space goal
F_ext = self.readGUIparams(ForceGUIids) # applied external forces
# get current joint states
q, dq, _ = self.getJointStates()
# Error in task space
x = self.solveForwardPositonKinematics(q)
x_e = xd - x
dx = self.solveForwardVelocityKinematics(q, dq)
dx_e = dxd - dx
# Task space dynamics
# Jacobian
J = self.getJacobian(q)
J_inv = np.linalg.pinv(J)
# Inertia matrix in the joint space
Mq, G, _ = self.calculateDynamicMatrices()
# Inertia matrix in the task space
Mx = np.dot(np.dot(np.transpose(J_inv), Mq), J_inv)
# Force in task space
Fx = np.dot(np.dot(np.linalg.inv(Md), Mx),
(np.dot(Kp, x_e) + np.dot(Kd, dx_e)))
# External Force applied
F_w_ext = np.dot((np.dot(np.linalg.inv(Md), Mx) - np.eye(6)),
F_ext)
Fx += F_w_ext
# Force in joint space
Fq = np.dot(np.transpose(J), Fx)
# Controlled Torque
tau = G + Fq
# tau += kd * np.asarray(dq) # if joint damping is turned off, this torque will not be required
# print('tau:', tau)
# Activate torque control
p.setJointMotorControlArray(self.robot_id,
self.controllable_joints,
controlMode=p.TORQUE_CONTROL,
forces=tau)
p.stepSimulation()
time.sleep(self.time_step)
p.disconnect()
def __del__(self):
p.disconnect()
import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
p.setTimeStep(1/200)
planeID = p.loadURDF("plane.urdf")
carId = p.loadURDF("racecar/racecar.urdf", basePosition=[0,0,0])
position, orientation = p.getBasePositionAndOrientation(carId)
for _ in range(10000):
pos, ori = p.getBasePositionAndOrientation(carId)
p.applyExternalForce(carId, 0, [50, 0, 0], pos, p.WORLD_FRAME)
p.stepSimulation()
time.sleep(1 / 200)
p.disconnect(physicsClient)
pybullet.connect(pybullet.SHARED_MEMORY)
#load URDF, given a relative or absolute file+path
obj = pybullet.loadURDF("r2d2.urdf")
posX=0
posY=3
posZ=2
obj2 = pybullet.loadURDF("kuka_lwr/kuka.urdf",posX,posY,posZ)
#query the number of joints of the object
numJoints = pybullet.getNumJoints(obj)
print (numJoints)
#set the gravity acceleration
pybullet.setGravity(0,0,-9.8)
#step the simulation for 5 seconds
t_end = time.time() + 5
while time.time() < t_end:
pybullet.stepSimulation()
print ("finished")
#remove all objects
pybullet.resetSimulation()
#disconnect from the physics server
pybullet.disconnect()
def render(self, mode='human'):
""" Render Pybullet simulation """
p.disconnect(self.physics_client)
self.physics_client = p.connect(p.GUI)
self.create_world()
def _close(self): if (self.physicsClientId>=0): p.disconnect(self.physicsClientId) self.physicsClientId = -1
def __del__(self):
"""Clean up connection if not already done."""
try:
pybullet.disconnect(physicsClientId=self._client)
except pybullet.error:
pass
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-10)
p.disconnect()
def pybullet_physics():
data = np.load(f"data/swimmer/swimmer{NLINKS:02d}.npy")
_, totalsteps, _ = data.shape
p.connect(p.GUI)
p.setRealTimeSimulation(False)
swimmer = p.loadURDF(
f"data/swimmer/swimmer{NLINKS:02d}/swimmer{NLINKS:02d}.urdf",
flags=p.URDF_USE_INERTIA_FROM_FILE)
# Turn off zero-velocity holding motors.
for joint in range(p.getNumJoints(swimmer)):
p.setJointMotorControl2(swimmer, joint, p.VELOCITY_CONTROL, force=0)
traj = data[0, :, :]
torques = traj[:, :NJOINTS]
q0 = traj[0, NJOINTS:2 * NJOINTS]
for j, q in enumerate(q0):
p.resetJointState(swimmer, j + 3, q)
head_yaw = traj[0, 2 * NJOINTS + 2]
p.resetJointState(swimmer, 2, head_yaw)
dt = 1 / 500
p.setTimeStep(dt) # From mjcf
control_steps = 10 # per env
with open("pybullet_rollout.csv", "w") as f:
for timestep in tqdm(range(totalsteps)):
applied_torques = []
for joint in range(NJOINTS):
urdfjoint = joint + 3 # x + y + yaw
torque = torques[timestep, joint]
applied_torques.append(torque)
print(urdfjoint, p.getNumJoints(swimmer))
p.setJointMotorControl2(swimmer,
urdfjoint,
p.TORQUE_CONTROL,
force=torque)
dynamics_info = p.getDynamicsInfo(swimmer, urdfjoint)
print(dynamics_info[2], dynamics_info[3], dynamics_info[4])
state = []
for link in range(2, 2 + NLINKS):
link_state = p.getLinkState(swimmer, link)
state.append(link_state[0][0])
state.append(link_state[0][1])
mat = p.getMatrixFromQuaternion(link_state[1])
state.append(np.arctan2(-mat[0], mat[1]))
f.write(','.join(map(str, state)) + "\n")
for _ in range(control_steps):
p.stepSimulation()
joint_states = p.getJointStates(swimmer,
range(p.getNumJoints(swimmer)))
q = [s[0] for s in joint_states]
qd = [s[1] for s in joint_states]
print(f"{timestep:04d} tau: {applied_torques}")
print(f"{timestep:04d} q: {q}")
print(f"{timestep:04d} qd: {qd}")
time.sleep(args.slowdown / control_steps * dt)
p.disconnect()