def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() logger = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) with DisabledCollisionsContext(sim, [], []): ####### # LazyPRM # ####### # Use parametric linear interpolation with 10 steps between points. interp = partial(parametric_lerp, steps=10) # See params for PRM specific parameters prm = LazyPRM(state_space, svc, interp, params={ 'n_samples': 4000, 'k': 8, 'ball_radius': 2.5 }) logger.info("Planning....") plan = prm.plan( np.array([0, 0, 0, 0, 0, 0, 0]), np.array([ 1.5262755737449423, -0.1698540226273928, 2.7788151824762055, 2.4546623466066135, 0.7146948867821279, 2.7671787952787184, 2.606128412644311 ])) # get_path() reuses the interp function to get the path between vertices of a successful plan path = prm.get_path(plan) if len(path) == 0: logger.info("Planning failed....") sys.exit(1) logger.info("Plan found....") # splinging uses numpy so needs to be converted path = [np.array(p) for p in path] # Create a MinJerk spline trajectory using JointTrajectoryCurve and execute jtc = JointTrajectoryCurve() traj = jtc.generate_trajectory(path, move_time=2) sawyer_robot.execute_trajectory(traj) key = input("Press any key to excute plan.") try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() _ = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) ############ # PLANNING # ############ valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): while True: sample = state_space.sample() if svc.validate(sample): valid_samples.append(sample) if len(valid_samples) >= 1: break # Generate local plan between two points and execute local plan. steps = 100 move_time = 5 start_pos = [0] * 7 print(np.array(sawyer_robot.get_current_joint_states()[0:7])) print(np.array(valid_samples[0])) qt, qdt, qddt = interpolate_5poly(np.array(start_pos[0:7]), np.array(valid_samples[0]), steps) traj = list( zip([move_time * n / steps for n in range(0, steps)], [list(q) for q in qt])) print(traj) sawyer_robot.execute_trajectory(traj) # Loop until someone shuts us down try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() _ = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): while True: sample = state_space.sample() if svc.validate(sample): valid_samples.append(sample) if len(valid_samples) >= 1: break # Generate local plan between two points and execute local plan. start_pos = [0] * 7 sawyer_robot.move_to_joint_pos(start_pos) time.sleep(.1) # This interpolate_5poly will be replaced with the sequence of points generated by a planner. qt, _, _ = interpolate_5poly(np.array(start_pos[0:7]), np.array(valid_samples[0]), 50) jtc = JointTrajectoryCurve() traj = jtc.generate_trajectory(qt, move_time=2) sawyer_robot.execute_trajectory(traj) # Loop until someone shuts us down try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def main(): config = {} config["sim_objects"] = [{ "object_name": "Ground", "model_file_or_sim_id": "plane.urdf", "position": [0, 0, 0] }, { "object_name": "Table", "model_file_or_sim_id": ASSETS_PATH + 'table.sdf', "position": [0.0, 0, 0], "orientation": [0, 0, 1.5708] }] sim_context = SawyerSimContext(config) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() scs = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() table_obj = sim_context.get_sim_objects(names="Table")[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) n_samples = 1000 valid_samples = [] starttime = timeit.default_timer() # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): print("Sampling start time is :", starttime) for i in range(0, n_samples): sample = scs.sample() if svc.validate(sample): print(sample) valid_samples.append(sample) print("The time difference is :", timeit.default_timer() - starttime) print("{} valid of {}".format(len(valid_samples), n_samples)) p.disconnect()
def main(): sim_context = SawyerSimContext(setup=False) sim_context.setup(sim_overrides={"use_gui": False}) sim = sim_context.get_sim_instance() logger = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() sawyer_id = sawyer_robot.get_simulator_id() ground_plane = sim_context.get_sim_objects(['Ground'])[0] # Get the robot moving! sawyer_robot.move_to_joint_pos(target_position=[1] * 7) while sawyer_robot.check_if_at_position([1] * 7, 0.5) is False: p.stepSimulation() print('\n\n\n\n\n\n') # Get current states pos, vel, torq = getJointStates(sawyer_id) names, mpos, mvel, mtorq = getMotorJointStates(sawyer_id) print("Moving joint names:") print(names) sawyerEELink = get_joint_info_by_name(sawyer_id, 'right_l6').idx print(sawyerEELink) result = p.getLinkState(sawyer_id, sawyerEELink, 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(sawyer_id, sawyerEELink, com_trn, mpos, zero_vec, zero_vec) J = np.vstack([np.array(jac_t), np.array(jac_r)])[:, [0, 2, 3, 4, 5, 6, 7]] print("JACOBIAN") print(J) J_cross = np.dot(J.T, np.linalg.inv(np.dot(J, J.T))) print("PSEUDOINVERSE JACOBIAN") print(J_cross) print("Link linear position and velocity of CoM from getLinkState:") print(link_vt, link_vr) print( "Link linear and angular velocity of CoM from linearJacobian * q_dot:" ) print(np.dot(J, np.array(mvel)[[0, 2, 3, 4, 5, 6, 7]].T)) print() print() print("Link joint velocities of CoM from getLinkState:") print(np.array(mvel)[[0, 2, 3, 4, 5, 6, 7]]) print( "Link q given link linear position and velocity using J_cross * x_dot." ) x = np.hstack([np.array(link_vt), np.array(link_vr)]) q = np.dot(J_cross, np.array(x).T) print(q)
['right_gripper_l_finger_joint', (0.0, 0.020833)], ['right_gripper_r_finger_joint', (-0.020833, 0.0)], ['head_pan', (-5.0952, 0.9064)]] ############################################# # Import Serialized LfD Graph # ############################################# with open( os.path.dirname(os.path.abspath(__file__)) + "/serialization_test.json", "r") as f: serialized_data = json.load(f) config = serialized_data["config"] intermediate_trajectories = serialized_data["intermediate_trajectories"] keyframes = OrderedDict( sorted(serialized_data["keyframes"].items(), key=lambda t: int(t[0]))) sim_context = SawyerSimContext(None, setup=False, planning_context=None) sim_context.setup(sim_overrides={"run_parallel": False}) sim = sim_context.get_sim_instance() logger = sim_context.get_logger() sawyer_robot = sim_context.get_robot() svc = sim_context.get_state_validity() interp_fn = partial(parametric_lerp, steps=5) ############################################# # CREATE A PLANNING GRAPH # ############################################# # This will be used to generate a sequence # # of constrained motion plans. # ############################################# planning_G = nx.Graph()
def main(): configuration = {} configuration["sim"] = { "use_real_time": False, "use_gui": False, "run_parallel": True } configuration["logging"] = {"handlers": ['logging'], "level": "debug"} configuration["sawyer"] = { "robot_name": "sawyer0", "position": [0, 0, 0.9], "fixed_base": True } configuration["sim_objects"] = [ { "object_name": "Ground", "model_file_or_sim_id": "plane.urdf", "position": [0, 0, 0] }, # { # "object_name": "Table", # "model_file_or_sim_id": ASSETS_PATH + 'table.sdf', # "position": [0.7, 0, 0], # "orientation": [0, 0, 1.5708] # }, { "object_name": "cube0", "model_file_or_sim_id": "cube_small.urdf", "position": [0.75, 0, .55] }, { "object_name": "cube1", "model_file_or_sim_id": "cube_small.urdf", "position": [0.74, 0.05, .55] }, { "object_name": "cube2", "model_file_or_sim_id": "cube_small.urdf", "position": [0.67, -0.1, .55] }, { "object_name": "cube3", "model_file_or_sim_id": "cube_small.urdf", "position": [0.69, 0.1, .55] } ] num_workers = 8 worker_fn = partial(parallel_projection_worker, sim_context_cls=SawyerSimContext, sim_config=configuration) with Pool(num_workers) as p: multi_s = timer() results = p.map(worker_fn, [25, 25, 25, 25, 25, 25, 25, 25]) valid_samples = list(itertools.chain.from_iterable(results)) multi_e = timer() print("Number of valid samples: {}".format(len(valid_samples))) configuration["sim"] = { "use_real_time": True, "use_gui": True, "run_parallel": False } sim_context = SawyerSimContext(configuration) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() scs = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] for sample in valid_samples: world_pose, local_pose = sawyer_robot.solve_forward_kinematics(sample) trans, quat = world_pose[0], world_pose[1] print(trans, quat2rpy(quat)) sawyer_robot.move_to_joint_pos(list(sample)) while sawyer_robot.check_if_at_position(list(sample), 0.5) is False: time.sleep(0.1) pass time.sleep(1.5) # # Loop until someone shuts us down try: while True: pass # sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
################################## # Planning Context # ################################## config = {} config["sim_objects"] = [{ "object_name": "Ground", "model_file_or_sim_id": "plane.urdf", "position": [0, 0, 0] }, { "object_name": "Table", "model_file_or_sim_id": ASSETS_PATH + 'table.sdf', "position": [3.0, 0, 0], "orientation": [0, 0, 1.5708] }] sim_context = SawyerSimContext(config, setup=True, planning_context=None) sim = sim_context.get_sim_instance() logger = sim_context.get_logger() sawyer_robot = sim_context.get_robot() svc = sim_context.get_state_validity() interp_fn = partial(parametric_lerp, steps=5) ############################################# # Import Serialized LfD Graph # ############################################# with open( os.path.dirname(os.path.abspath(__file__)) + "/serialization_test.json", "r") as f: serialized_data = json.load(f) config = serialized_data["config"] intermediate_trajectories = serialized_data["intermediate_trajectories"]