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