def _setup_self_collision_fn(self, sawyer_robot): sawyer_id = self.sawyer_robot.get_simulator_id() excluded_pairs = [ (get_joint_info_by_name(sawyer_id, 'right_l6').idx, get_joint_info_by_name(sawyer_id, 'right_connector_plate_base').idx) ] link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs) return partial(self_collision_test, robot=sawyer_robot, link_pairs=link_pairs)
def _setup_collision_exclusions(self): ground_plane = self.get_sim_objects(['Ground'])[0] sawyer_id = self.sawyer_robot.get_simulator_id() # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] self.collision_exclusions = { "excluded_bodies": excluded_bodies, "excluded_body_link_pairs": excluded_body_link_pairs }
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)
def main(): ################################ # Environment Checks and Flags # ################################ if os.environ.get('ROS_DISTRO'): rospy.init_node("CAIRO_Sawyer_Simulator") use_ros = True else: use_ros = False ######################################################## # Create the Simulator and pass it a Logger (optional) # ######################################################## logger = Logger() sim = Simulator(logger=logger, use_ros=use_ros, use_gui=True, use_real_time=True) # Initialize the Simulator ##################################### # Create a Robot, or two, or three. # ##################################### sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1) ############################################# # Create sim environment objects and assets # ############################################# ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0]) sawyer_id = sawyer_robot.get_simulator_id() sawyer_robot.move_to_joint_pos([ 0.006427734375, -0.4784267578125, -2.6830537109375, -1.5901376953125, 0.1734560546875, 1.1468447265625, 1.310236328125 ]) time.sleep(3) # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # PLANNING # ############ # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ######################### # STATE SPACE SELECTION # ######################### # This inherently uses UniformSampler but a different sampling class could be injected. state_space = SawyerConfigurationSpace() ############################## # STATE VALIDITY FORMULATION # ############################## # Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision. excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx), (get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)] link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs) self_collision_fn = partial(self_collision_test, robot=sawyer_robot, link_pairs=link_pairs) # Create constraint checks def constraint_test(q): upright_orientation = [ 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838 ] axis = 'z' threshold = 15 world_pose, _ = sawyer_robot.solve_forward_kinematics(q) return orientation(upright_orientation, world_pose[1], threshold, axis) # In this case, we only have a self_col_fn. svc = StateValidityChecker(self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test]) ####### # PRM # ####### # Use parametric linear interpolation with 5 steps between points. interp = partial(parametric_lerp, steps=5) # See params for PRM specific parameters prm = PRM(state_space, svc, interp, params={ 'n_samples': 1000, 'k': 10, 'ball_radius': 3.0 }) logger.info("Planning....") plan = prm.plan( np.array([ 0.006427734375, -0.4784267578125, -2.6830537109375, -1.5901376953125, 0.1734560546875, 1.1468447265625, 1.310236328125 ]), np.array([ -0.9232412109375, 0.2353603515625, -2.51373828125, -0.6898984375, 0.33058203125, 1.0955361328125, 1.14510546875 ])) logger.info("Plan found....") print(len(plan)) print(plan) # 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) ########## # SPLINE # ########## # 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=5) print("Executing splined trajectory...") sawyer_robot.execute_trajectory(traj) try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def parallel_sample_worker(num_samples): ######################################################## # Create the Simulator and pass it a Logger (optional) # ######################################################## logger = Logger() if not Simulator.is_instantiated(): logger.warn("Simulator not instantiated, doing so now...") sim = Simulator(logger=logger, use_ros=False, use_gui=False, use_real_time=True) # Initialize the Simulator else: sim = Simulator.get_instance() ##################################### # Create a Robot, or two, or three. # ##################################### sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1) ############################################# # Create sim environment objects and assets # ############################################# ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0]) sawyer_id = sawyer_robot.get_simulator_id() # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # SAMPLING # ############ valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ######################### # STATE SPACE SELECTION # ######################### # This inherently uses UniformSampler but a different sampling class could be injected. state_space = SawyerConfigurationSpace() ############################## # STATE VALIDITY FORMULATION # ############################## # Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision. excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx), (get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)] link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs) self_collision_fn = partial( self_collision_test, robot=sawyer_robot, link_pairs=link_pairs) # Create constraint checks # def constraint_test(q): # upright_orientation = [ # 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838] # axis = 'z' # threshold = 15 # world_pose, _ = sawyer_robot.solve_forward_kinematics(q) # return orientation(upright_orientation, world_pose[1], threshold, axis) # In this case, we only have a self_col_fn. svc = StateValidityChecker( self_col_func=self_collision_fn, col_func=None, validity_funcs=None) # svc = StateValidityChecker( # self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test]) count = 0 while count < num_samples: q_rand = np.array(state_space.sample()) if svc.validate(q_rand): valid_samples.append(q_rand) count += 1 return valid_samples
use_real_time=True) # Initialize the Simulator ##################################### # Create a Robot, or two, or three. # ##################################### sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1) sawyer_id = sawyer_robot.get_simulator_id() ############################################# # Create sim environment objects and assets # ############################################# ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0]) # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # PLANNING # ############ # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ######################### # STATE SPACE SELECTION # ######################### # This inherently uses UniformSampler but a different sampling class could be injected. state_space = SawyerConfigurationSpace() ##############################
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_id = sawyer_robot.get_simulator_id() ground_plane = sim_context.get_sim_objects(['Ground'])[0] # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # PLANNING # ############ # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ####### # PRM # ####### # Use parametric linear interpolation with 10 steps between points. interp = partial(parametric_lerp, steps=10) # See params for PRM specific parameters prm = PRM(state_space, svc, interp, params={ 'n_samples': 3000, 'k': 6, 'ball_radius': 2.0 }) 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....") print(len(path)) ########## # SPLINE # ########## # 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=10) sawyer_robot.execute_trajectory(traj) try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)