def test_drake_base_motion(pr2, base_start, base_goal, obstacles=[]):
# TODO: combine this with test_arm_motion
"""
Drake's PR2 URDF has explicit base joints
"""
disabled_collisions = get_disabled_collisions(pr2)
base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']]
set_joint_positions(pr2, base_joints, base_start)
base_joints = base_joints[:2]
base_goal = base_goal[:len(base_joints)]
wait_for_user('Plan Base?')
base_path = plan_joint_motion(pr2,
base_joints,
base_goal,
obstacles=obstacles,
disabled_collisions=disabled_collisions)
if base_path is None:
print('Unable to find a base path')
return
print(len(base_path))
for bq in base_path:
set_joint_positions(pr2, base_joints, bq)
if SLEEP is None:
wait_for_user('Continue?')
else:
time.sleep(SLEEP)
def fn(conf1, conf2, fluents=[]):
if teleport is True:
path = [conf1, conf2]
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
# TODO: double check that collisions with movable obstacles are considered
obstacles = list(obstacle_from_name.values())
attachments = parse_fluents(robot, brick_from_index, fluents,
obstacles)
set_joint_positions(robot, movable_joints, conf1)
path = plan_joint_motion(
robot,
movable_joints,
conf2,
obstacles=obstacles,
attachments=attachments,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
#weights=weights, resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
return None
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
def main(group=SE3):
connect(use_gui=True)
set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.]))
# TODO: can also create all links and fix some joints
# TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed
obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED)
#robot = create_cylinder(radius=0.025, height=0.1, color=BLUE)
obstacles = [obstacle]
collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE)
robot = create_flying_body(group, collision_id, visual_id)
body_link = get_links(robot)[-1]
joints = get_movable_joints(robot)
joint_from_group = dict(zip(group, joints))
print(joint_from_group)
#print(get_aabb(robot, body_link))
dump_body(robot, fixed=False)
custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()}
# sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
# for i in range(10):
# conf = sample_fn()
# set_joint_positions(robot, joints, conf)
# wait_for_user('Iteration: {}'.format(i))
# return
initial_point = SIZE*np.array([-1., -1., 0])
#initial_point = -1.*np.ones(3)
final_point = -initial_point
initial_euler = np.zeros(3)
add_line(initial_point, final_point, color=GREEN)
initial_conf = np.concatenate([initial_point, initial_euler])
final_conf = np.concatenate([final_point, initial_euler])
set_joint_positions(robot, joints, initial_conf)
#print(initial_point, get_link_pose(robot, body_link))
#set_pose(robot, Pose(point=-1.*np.ones(3)))
path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles,
self_collisions=False, custom_limits=custom_limits)
if path is None:
disconnect()
return
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
point, quat = get_link_pose(robot, body_link)
#euler = euler_from_quat(quat)
euler = intrinsic_euler_from_quat(quat)
print(conf)
print(point, euler)
wait_for_user('Step: {}/{}'.format(i, len(path)))
wait_for_user('Finish?')
disconnect()
def wild_gen_fn(name, conf1, conf2, *args):
is_initial = (conf1.element is None) and (conf2.element is not None)
is_goal = (conf1.element is not None) and (conf2.element is None)
if is_initial:
supporters = []
elif is_goal:
supporters = list(element_bodies)
else:
supporters = [conf1.element
] # TODO: can also do according to levels
retrace_supporters(conf1.element, incoming_supporters, supporters)
element_obstacles = {element_bodies[e] for e in supporters}
obstacles = set(static_obstacles) | element_obstacles
if not collisions:
obstacles = set()
robot = index_from_name(robots, name)
conf1.assign()
joints = get_movable_joints(robot)
# TODO: break into pieces at the furthest part from the structure
weights = JOINT_WEIGHTS
resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
disabled_collisions = get_disabled_collisions(robot)
#path = [conf1, conf2]
path = plan_joint_motion(robot,
joints,
conf2.positions,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
weights=weights,
resolutions=resolutions,
restarts=3,
iterations=100,
smooth=100)
if not path:
return
path = [conf1.positions] + path[1:-1] + [conf2.positions]
traj = MotionTrajectory(robot, joints, path)
command = Command([traj])
edges = [
(conf1, command, conf2),
(conf2, command, conf1), # TODO: reverse
]
outputs = []
#outputs = [(command,)]
facts = []
for q1, cmd, q2 in edges:
facts.extend([
('Traj', name, cmd),
('CTraj', name, cmd),
('MoveAction', name, q1, q2, cmd),
])
yield WildOutput(outputs, facts)
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
user_input('Plan Arm?')
arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions)
if arm_path is None:
print('Unable to find an arm path')
return
print(len(arm_path))
for q in arm_path:
set_joint_positions(pr2, left_joints, q)
#raw_input('Continue?')
time.sleep(0.01)
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
wait_if_gui('Plan Arm?')
with LockRenderer(lock=False):
arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions)
if arm_path is None:
print('Unable to find an arm path')
return
print(len(arm_path))
for q in arm_path:
set_joint_positions(pr2, left_joints, q)
#wait_if_gui('Continue?')
wait_for_duration(0.01)
def plan_motion(robot, joints, goal_positions, attachments=[], obstacles=None, holonomic=False, reversible=False, **kwargs):
attached_bodies = [attachment.child for attachment in attachments]
moving_bodies = [robot] + attached_bodies
if obstacles is None:
obstacles = get_bodies()
obstacles = OrderedSet(obstacles) - set(moving_bodies)
if holonomic:
return plan_joint_motion(robot, joints, goal_positions,
attachments=attachments, obstacles=obstacles, **kwargs)
# TODO: just sample the x, y waypoint and use the resulting orientation
# TODO: remove overlapping configurations/intervals due to circular joints
return plan_nonholonomic_motion(robot, joints, goal_positions, reversible=reversible,
linear_tol=1e-6, angular_tol=0.,
attachments=attachments, obstacles=obstacles, **kwargs)
def test_drake_base_motion(pr2, base_start, base_goal):
# TODO: combine this with test_arm_motion
"""
Drake's PR2 URDF has explicit base joints
"""
disabled_collisions = get_disabled_collisions(pr2)
base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']]
set_joint_positions(pr2, base_joints, base_start)
user_input('Plan Base?')
base_path = plan_joint_motion(pr2, base_joints, base_goal, disabled_collisions=disabled_collisions)
if base_path is None:
print('Unable to find a base path')
return
print(len(base_path))
for bq in base_path:
set_joint_positions(pr2, base_joints, bq)
user_input('Continue?')
def fn(bq, aq1, aq2, fluents=[]):
#if aq1 == aq2:
# return None
bq.assign()
aq1.assign()
attachments, obstacles = parse_fluents(world, fluents)
obstacles.update(world.static_obstacles)
if not collisions:
obstacles = set()
robot_saver = BodySaver(world.robot)
if teleport:
path = [aq1.values, aq2.values]
else:
path = plan_joint_motion(
world.robot,
aq2.joints,
aq2.values,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions,
restarts=2,
iterations=50,
smooth=50)
if path is None:
print('Failed to find an arm motion plan for {}->{}'.format(
aq1, aq2))
if PAUSE_MOTION_FAILURES:
set_renderer(enable=True)
print(fluents)
for bq in [aq1, aq2]:
bq.assign()
wait_for_user()
set_renderer(enable=False)
return None
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
Trajectory(world, world.robot, world.arm_joints,
path),
],
name='arm')
return (cmd, )
def step(self, goal_pos, goal_orien, fingers):
ll = [-2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671]
ul = [2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671]
jr = [5.8, 3.6, 5.8, 2.9, 5.8, 3.8, 5.8]
rp = [0, -0.215, 0, -2.57, 0, 2.356, 2.356]
jointPoses = p.calculateInverseKinematics(self.pandaUid, 11, goal_pos,
goal_orien, ll, ul, jr,
rp)[0:7]
# print('joint poses : ', jointPoses)
obstacles = [self.tableUid, self.objectUid[0], self.objectUid[1]]
path_conf = plan_joint_motion(self.pandaUid,
list(range(7)),
jointPoses,
obstacles=obstacles,
algorithm='birrt')
# self.render()
# path_conf = plan_joint_motion(self.pandaUid, list(range(7)), jointPoses, max_distance=1e-5,
# obstacles=obstacles)
# if path_conf is None:
# print('Unable to find a path')
# return
if path_conf is not None:
for q in path_conf:
self.render()
# set_joint_positions(self.pandaUid, list(range(7))+[9,10], list(q)+[0.04,0.04])
p.setJointMotorControlArray(self.pandaUid,
list(range(7)) + [9, 10],
p.POSITION_CONTROL,
list(q) + 2 * [fingers])
p.stepSimulation()
print('this is the distance : ', self.observation())
reward = 0 # not use reward var
done = False
return np.array(self.get_panda_position()).astype(
np.float32), reward, done, self.observation()
def gen_fn(arm, conf1, conf2, fluents=[]):
arm_confs, object_grasps, object_poses, contains_liquid = parse_fluents(
world, fluents)
for a, q in arm_confs.items():
#print(a, q) # TODO: some optimistic values are getting through
set_joint_positions(world.robot, get_arm_joints(world.robot, a), q)
for name, pose in object_poses.items():
set_pose(world.bodies[name], pose)
obstacle_names = list(world.get_fixed()) + list(object_poses)
obstacles = [world.bodies[name] for name in obstacle_names]
attachments = {}
holding_water = None
water_attachment = None
for arm2, (obj, grasp) in object_grasps.items():
set_gripper_position(world.robot, arm, grasp.grasp_width)
attachment = get_grasp_attachment(world, arm2, grasp)
attachment.assign()
if arm == arm2: # The moving arm
if obj in contains_liquid:
holding_water = obj
water_attachment = attachment
attachments[obj] = attachment
else: # The stationary arm
obstacles.append(world.get_body(obj))
if not collisions:
obstacles = []
# TODO: dynamically adjust step size to be more conservative near longer movements
arm_joints = get_arm_joints(world.robot, arm)
set_joint_positions(world.robot, arm_joints, conf1)
weights, resolutions = get_weights_resolutions(world.robot, arm)
#print(holding_water, attachments, [get_body_name(body) for body in obstacles])
if teleport:
path = [conf1, conf2]
elif holding_water is None:
# TODO(caelan): unify these two methods
path = plan_joint_motion(world.robot,
arm_joints,
conf2,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
restarts=5,
iterations=50,
smooth=smooth)
else:
reference_pose = (unit_point(), get_liquid_quat(holding_water))
path = plan_water_motion(world.robot,
arm_joints,
conf2,
water_attachment,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
reference_pose=reference_pose,
restarts=7,
iterations=75,
smooth=smooth)
if path is None:
return None
control = Control({
'action':
'move-arm',
#'objects': [],
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments=attachments),
'commands': [
ArmTrajectory(arm, path),
],
})
return (control, )
def plan_approach(world,
approach_pose,
attachments=[],
obstacles=set(),
teleport=False,
switches_only=False,
approach_path=not MOVE_ARM,
**kwargs):
# TODO: use velocities in the distance function
distance_fn = get_distance_fn(world.robot, world.arm_joints)
aq = world.carry_conf
grasp_conf = get_joint_positions(world.robot, world.arm_joints)
if switches_only:
return [aq.values, grasp_conf]
# TODO: could extract out collision function
# TODO: track the full approach motion
full_approach_conf = world.solve_inverse_kinematics(
approach_pose, nearby_tolerance=NEARBY_APPROACH)
if full_approach_conf is None: # TODO: | {obj}
if PRINT_FAILURES: print('Pregrasp kinematic failure')
return None
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
#robot_obstacle = world.robot
if any(pairwise_collision(robot_obstacle, b)
for b in obstacles): # TODO: | {obj}
if PRINT_FAILURES: print('Pregrasp collision failure')
return None
approach_conf = get_joint_positions(world.robot, world.arm_joints)
if teleport:
return [aq.values, approach_conf, grasp_conf]
distance = distance_fn(grasp_conf, approach_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print('Pregrasp proximity failure (distance={:.5f})'.format(
distance))
return None
resolutions = ARM_RESOLUTION * np.ones(len(world.arm_joints))
grasp_path = plan_direct_joint_motion(
world.robot,
world.arm_joints,
grasp_conf,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions / 4.)
if grasp_path is None:
if PRINT_FAILURES: print('Pregrasp path failure')
return None
if not approach_path:
return grasp_path
# TODO: plan one with attachment placed and one held
# TODO: can still use this as a witness that the conf is reachable
aq.assign()
approach_path = plan_joint_motion(
world.robot,
world.arm_joints,
approach_conf,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions,
restarts=2,
iterations=25,
smooth=25)
if approach_path is None:
if PRINT_FAILURES: print('Approach path failure')
return None
return approach_path + grasp_path
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
