def go_to_conf(self, conf):
control_joints(
self.pw.robot,
get_movable_joints(self.pw.robot) + list(self.finger_joints),
tuple(conf) + (self.target_grip, self.target_grip))
simulate_for_duration(0.3)
self.steps_taken += 0.3
def do_setup(self):
self.change_grip(0.025, force=0)
self.approach()
self.change_grip(0.005, force=0.8) # force control this. 1 was ok
simulate_for_duration(0.1)
self.place()
simulate_for_duration(0.5)
self.steps_taken += 0.5
self.pw.shift_t_joint(self._shift, 0)
self.save_state()
def squeeze(self, force, width=None):
self.squeeze_force = force
diffs = deque(maxlen=5)
k = 0.00058
kp = 0 #0.00001# 0.00015
n_tries = 400
tol = 0.1
for i in range(n_tries):
if width is None:
curr_force = self.get_gripper_force()
diff = force - curr_force
#print("diff", diff)
diffs.append(curr_force)
if len(diffs) == 2:
deriv_diff = diffs[1] - diffs[0]
else:
deriv_diff = 0
curr_pos = p.getJointState(self.pw.robot,
self.finger_joints[0])[0]
target = curr_pos - (k * diff + kp * (deriv_diff))
else:
target = width
control_joints(self.pw.robot, self.finger_joints, (target, target))
self.target_grip = target
if width is not None:
simulate_for_duration(0.5)
self.pw.steps_taken += 0.5
if self.pw.steps_taken >= self.total_timeout:
return
break
else:
simulate_for_duration(0.1) #less sure of it
self.pw.steps_taken += 0.1
if self.pw.steps_taken >= self.total_timeout:
return
if len(diffs) > 3 and abs(diff) < tol and abs(
np.mean(list(diffs)[-3:]) - force) < tol:
#print("Reached target force")
break
if n_tries == i - 1:
print("Failure to reach", diff)
def main():
connect(use_gui=True)
add_data_path()
set_camera(0, -30, 1)
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
cup = load_model('models/cup.urdf', fixed_base=True)
#set_point(cup, Point(z=stable_z(cup, plane)))
set_point(cup, Point(z=.2))
set_color(cup, (1, 0, 0, .4))
num_droplets = 100
#radius = 0.025
#radius = 0.005
radius = 0.0025
# TODO: more efficient ways to make all of these
droplets = [create_sphere(radius, mass=0.01)
for _ in range(num_droplets)] # kg
cup_thickness = 0.001
lower, upper = get_lower_upper(cup)
print(lower, upper)
buffer = cup_thickness + radius
lower = np.array(lower) + buffer * np.ones(len(lower))
upper = np.array(upper) - buffer * np.ones(len(upper))
limits = zip(lower, upper)
x_range, y_range = limits[:2]
z = upper[2] + 0.1
#x_range = [-1, 1]
#y_range = [-1, 1]
#z = 1
for droplet in droplets:
x = np.random.uniform(*x_range)
y = np.random.uniform(*y_range)
set_point(droplet, Point(x, y, z))
for i, droplet in enumerate(droplets):
x, y = np.random.normal(0, 1e-3, 2)
set_point(droplet, Point(x, y, z + i * (2 * radius + 1e-3)))
#dump_world()
wait_for_user()
#user_input('Start?')
enable_gravity()
simulate_for_duration(5.0)
# enable_real_time()
# try:
# while True:
# enable_gravity() # enable_real_time requires a command
# #time.sleep(dt)
# except KeyboardInterrupt:
# pass
# print()
#time.sleep(1.0)
wait_for_user('Finish?')
disconnect()
def go_to_pose(self,
target_pose,
obstacles=[],
attachments=[],
cart_traj=False,
use_policy=False,
maxForce=100):
total_traj = []
if self.pw.handonly:
p.changeConstraint(self.pw.cid,
target_pose[0],
target_pose[1],
maxForce=maxForce)
for i in range(80):
simulate_for_duration(self.dt_pose)
self.pw.steps_taken += self.dt_pose
if self.pw.steps_taken >= self.total_timeout:
return total_traj
ee_loc = p.getBasePositionAndOrientation(self.pw.robot)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
total_traj.append(ee_loc)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
for i in range(50):
end_conf = inverse_kinematics_helper(self.pw.robot,
self.ee_link, target_pose)
if not use_policy:
motion_plan = plan_joint_motion(self.pw.robot,
get_movable_joints(
self.pw.robot),
end_conf,
obstacles=obstacles,
attachments=attachments)
if motion_plan is not None:
for conf in motion_plan:
self.go_to_conf(conf)
ee_loc = p.getLinkState(self.pw.robot, 8)
if cart_traj:
total_traj.append(ee_loc[0] + ee_loc[1])
else:
total_traj.append(conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
ee_loc = p.getLinkState(self.pw.robot, 8)
next_loc = self.policy.predict(
np.array(ee_loc[0] + ee_loc[1]).reshape(1, 7))[0]
next_pos = next_loc[0:3]
next_quat = next_loc[3:]
next_conf = inverse_kinematics_helper(
self.pw.robot, self.ee_link, (next_pos, next_quat))
if cart_traj:
total_traj.append(next_loc)
else:
total_traj.append(next_conf)
self.go_to_conf(next_conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
ee_loc = p.getLinkState(self.pw.robot, 8)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
return total_traj
