def get_picknplace_robot_data(): MODEL_DIR = coop_assembly.get_data('models') robot_urdf = os.path.join(MODEL_DIR, ROBOT_URDF) robot_srdf = os.path.join(MODEL_DIR, ROBOT_SRDF) workspace_urdf = os.path.join(MODEL_DIR, WS_URDF) workspace_srdf = os.path.join(MODEL_DIR, WS_SRDF) move_group = None robot_model = RobotModel.from_urdf_file(robot_urdf) robot_semantics = RobotSemantics.from_srdf_file(robot_srdf, robot_model) robot = RobotClass(robot_model, semantics=robot_semantics) base_link_name = robot.get_base_link_name(group=move_group) # ee_link_name = robot.get_end_effector_link_name(group=move_group) ee_link_name = None # set to None since end effector is not included in the robot URDF, but attached later ik_joint_names = robot.get_configurable_joint_names(group=move_group) disabled_self_collision_link_names = robot_semantics.get_disabled_collisions( ) tool_root_link_name = robot.get_end_effector_link_name(group=move_group) workspace_model = RobotModel.from_urdf_file(workspace_urdf) workspace_semantics = RobotSemantics.from_srdf_file( workspace_srdf, workspace_model) workspace_robot_disabled_link_names = workspace_semantics.get_disabled_collisions( ) workspace_robot_disabled_link_names = [] return (robot_urdf, base_link_name, tool_root_link_name, ee_link_name, ik_joint_names, disabled_self_collision_link_names), \ (workspace_urdf, workspace_robot_disabled_link_names)
def test_panda_srdf_file(panda_srdf, panda_urdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) assert sorted(semantics.group_names) == sorted( ['panda_arm', 'hand', 'panda_arm_hand']) assert semantics.main_group_name == 'panda_arm_hand' assert semantics.get_base_link_name('panda_arm') == 'panda_link0' assert semantics.get_end_effector_link_name('panda_arm') == 'panda_link8' assert semantics.get_configurable_joint_names('panda_arm') == [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7' ] all_configurable_joint_names = [ j.name for j in semantics.get_all_configurable_joints() ] assert all_configurable_joint_names == [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1' ] configurable_joints = semantics.get_configurable_joints('panda_arm_hand') assert [j.type for j in configurable_joints] == [0, 0, 0, 0, 0, 0, 0, 2] set_joints = set(semantics.group_states['panda_arm']['ready'].keys()) assert set_joints == { 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7' }
def abb_irb4600_40_255_setup(): HERE = os.path.dirname(__file__) data_dir = os.path.abspath(os.path.join(HERE, "..", "..", "data", 'robots')) urdf_filename = os.path.join(data_dir, 'abb_irb4600_40_255', "urdf", "abb_irb4600_40_255.urdf") srdf_filename = os.path.join(data_dir, 'abb_fixed_waam', "srdf", "abb_irb4600_40_255.srdf") model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) return urdf_filename, semantics
def test_panda_srdf_file(panda_srdf, panda_urdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) assert semantics.group_names == ['panda_arm', 'hand', 'panda_arm_hand'] assert semantics.main_group_name == 'panda_arm' assert semantics.get_base_link_name() == 'panda_link0' assert semantics.get_end_effector_link_name() == 'panda_link8' assert semantics.get_configurable_joint_names() == [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7' ]
def rfl_setup(): HERE = os.path.dirname(__file__) data_dir = os.path.abspath(os.path.join(HERE, "..", "..", "data", 'robots')) urdf_filename = os.path.join(data_dir, 'rfl_description', 'rfl_description', "urdf", "rfl_pybullet.urdf") srdf_filename = os.path.join(data_dir, 'rfl_description', 'rfl_description', "urdf", "rfl.srdf") model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) return urdf_filename, semantics
def load_semantics(self, robot, srdf_filename): """Loads the semantic information of a robot. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot to be saved for use with PyBullet. srdf_filename : :obj:`str` or file object Absolute file path to the srdf file name. """ cached_robot = self.get_cached_robot(robot) robot.semantics = RobotSemantics.from_srdf_file( srdf_filename, cached_robot) self.disabled_collisions = robot.semantics.disabled_collisions
def Robot(client=None, load_geometry=False): """Returns a UR5 robot. This method serves mainly as help for writing examples, so that the code can stay short. It is intentionally capitalized to act as an almost drop-in replacement for the constructor of :class:`compas_fab.robots.Robot`. Parameters ---------- client: object Backend client. load_geometry: bool Returns ------- :class:`compas_fab.robots.Robot` Newly created instance of a UR5 robot. Examples -------- >>> from compas_fab.robots.ur5 import Robot >>> robot = Robot() >>> robot.name 'ur5' """ urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) if load_geometry: loader = LocalPackageMeshLoader(compas_fab.get('universal_robot'), 'ur_description') model.load_geometry(loader) robot = RobotClass(model, semantics=semantics) if client: robot.client = client return robot
def fixed_waam_setup(): HERE = os.path.dirname(__file__) package_path = os.path.abspath( os.path.join(HERE, "..", "..", "data", "robots", "abb_fixed_waam")) urdf_filename = os.path.join(package_path, "urdf", "abb_fixed_waam.urdf") srdf_filename = os.path.join(package_path, "srdf", "abb_fixed_waam.srdf") model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) tool_frame_robotA = Frame.from_euler_angles( [0.591366, -0.000922, 1.570177], static=True, axes='xyz', point=[-0.002241, -0.000202, 0.505922]) tool_mesh_robotA = Mesh.from_obj( os.path.join(package_path, "meshes", "collision", "waam_tool.obj")) robotA_tool = Tool(tool_mesh_robotA, tool_frame_robotA, collision=tool_mesh_robotA) return urdf_filename, semantics, robotA_tool
def test_passive_joints(panda_srdf, panda_urdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) assert ['panda_finger_joint2'] == semantics.passive_joints
def test_end_effectors(panda_srdf, panda_urdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) assert ['panda_link8'] == semantics.end_effectors
def test_disabled_collisions(panda_srdf, panda_urdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) assert ('panda_hand', 'panda_link6') in semantics.disabled_collisions assert ('panda_hand', 'panda_leftfinger') in semantics.disabled_collisions
def main(): choreo_problem_instance_dir = compas_fab.get('choreo_instances') result_save_path = os.path.join(choreo_problem_instance_dir, 'results', 'choreo_result.json') with open(result_save_path, 'r') as f: json_data = json.loads(f.read()) traj_time_cnt = 0.0 max_jt_vel = 0.2 max_jt_acc = 0.1 last_jt_pt = None # pybullet traj preview settings pybullet_preview = True PB_VIZ_CART_TIME_STEP = 0.05 PB_VIZ_TRANS_TIME_STEP = 0.04 PB_VIZ_PER_CONF_SIM = False urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03.obj') # [0.0, -94.94770102010436, 98.0376624092449, -93.01855212389889, 0.0, 0.0] # UR=192.168.0.30, Linux=192.168.0.1, Windows=192.168.0.2 # the following host IP should agree with the Linux machine host_ip = '192.168.0.1' if REAL_EXECUTION else 'localhost' with RosClient(host=host_ip, port=9090) as client: client.on_ready( lambda: print('Is ROS connected?', client.is_connected)) # get current configuration listener = roslibpy.Topic(client, JOINT_TOPIC_NAME, 'sensor_msgs/JointState') msg_getter = MsgGetter() listener.subscribe(msg_getter.receive_msg) time.sleep(2) last_seen_state = msg_getter.get_msg() print('current jt state: {}'.format(last_seen_state['position'])) model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics, client=client) group = robot.main_group_name joint_names = robot.get_configurable_joint_names() # base_link_name = robot.get_base_link_name() ee_link_name = robot.get_end_effector_link_name() if pybullet_preview: from conrob_pybullet import connect from compas_fab.backends.ros.plugins_choreo import display_trajectory_chunk from compas_fab.backends.pybullet import attach_end_effector_geometry, \ convert_mesh_to_pybullet_body, create_pb_robot_from_ros_urdf connect(use_gui=True) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, ee_link_name=ee_link_name) ee_meshes = [Mesh.from_obj(ee_filename)] ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) st_conf = Configuration.from_revolute_values( last_seen_state['position']) goal_conf = Configuration.from_revolute_values( json_data[0]['place2pick']['start_configuration']['values']) goal_constraints = robot.constraints_from_configuration( goal_conf, [math.radians(1)] * 6, group) init_traj_raw = robot.plan_motion(goal_constraints, st_conf, group, planner_id='RRTStar') init_traj = traj_reparam(init_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, init_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) print('************\nexecuting init transition') exec_jt_traj(client, joint_names, init_traj) print('executed?') input() for seq_id, e_process_data in enumerate(json_data): print('************\nexecuting #{} picknplace process'.format( seq_id)) # open gripper gripper_srv_call(client, state=0) print( '=====\nexecuting #{} place-retreat to pick-approach transition process' .format(seq_id)) traj_data = e_process_data['place2pick'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('=====\nexecuting #{} pick-approach to pick-grasp process'. format(seq_id)) traj_data = e_process_data['pick_approach'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() # close gripper gripper_srv_call(client, state=1) print('=====\nexecuting #{} pick-grasp to pick-retreat process'. format(seq_id)) traj_data = e_process_data['pick_retreat'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() print( '=====\nexecuting #{} pick-retreat to place-approach transition process' .format(seq_id)) traj_data = e_process_data['pick2place'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() print('=====\nexecuting #{} place-approach to place-grasp process'. format(seq_id)) traj_data = e_process_data['place_approach'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) # open gripper gripper_srv_call(client, state=0) print('executed?') input() print('=====\nexecuting #{} place-grasp to place-retreat process'. format(seq_id)) traj_data = e_process_data['place_retreat'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() if 'return2idle' in json_data[-1]: print('=====\nexecuting #{} return-to-idle transition process'. format(seq_id)) traj_data = e_process_data['return2idle'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj)
def panda_joints(panda_urdf, panda_srdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) return semantics.get_configurable_joints()
def sequenced_picknplace_plan( assembly_json_path, robot_model='ur3', pick_from_same_rack=True, customized_sequence=[], from_seq_id=0, to_seq_id=None, num_cart_steps=10, enable_viewer=True, plan_transit=True, transit_res=0.01, view_ikfast=False, tcp_tf_list=[1e-3 * 80.525, 0, 0], robot_start_conf=[0, -1.65715, 1.71108, -1.62348, 0, 0], scale=1, result_save_path='', sim_traj=True, cart_ts=0.1, trans_ts=0.01, per_conf_step=False, step_sim=False): # parser.add_argument('-vik', '--view_ikfast', action='store_true', help='Visualize each ikfast solutions') # parser.add_argument('-per_conf_step', '--per_conf_step', action='store_true', help='stepping each configuration in simulation') # transition motion planner settings RRT_RESTARTS = 5 RRT_ITERATIONS = 40 # rescaling # TODO: this should be done when the Assembly object is made unit_geos, static_obstacles = load_assembly_package(assembly_json_path, scale=scale) # urdf, end effector settings if robot_model == 'ur3': urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur3.urdf') # urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur3_collision_viz.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur3_moveit_config/config/ur3.srdf') else: urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'dms_2019_gripper/collision/190907_Gripper_05.obj') # geometry file is not loaded here model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics) base_link_name = robot.get_base_link_name() ee_link_name = robot.get_end_effector_link_name() ik_joint_names = robot.get_configurable_joint_names() disabled_link_names = semantics.get_disabled_collisions() # parse end effector mesh ee_meshes = [Mesh.from_obj(ee_filename)] tcp_tf = Translation(tcp_tf_list) # add static collision obstacles co_dict = {} for i, static_obs_mesh in enumerate(static_obstacles): cm = CollisionMesh(static_obs_mesh, 'so_' + str(i)) co_dict[cm.id] = {} co_dict[cm.id]['meshes'] = [cm.mesh] co_dict[cm.id]['mesh_poses'] = [cm.frame] # ====================================================== # ====================================================== # start pybullet environment & load pybullet robot connect(use_gui=enable_viewer) camera_base_pt = (0, 0, 0) camera_pt = np.array(camera_base_pt) + np.array([1, 0, 0.5]) set_camera_pose(tuple(camera_pt), camera_base_pt) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, ee_link_name=ee_link_name) ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) # update current joint conf and attach end effector pb_ik_joints = joints_from_names(pb_robot, ik_joint_names) pb_end_effector_link = link_from_name(pb_robot, ee_link_name) set_joint_positions(pb_robot, pb_ik_joints, robot_start_conf) for e_at in ee_attachs: e_at.assign() # draw TCP frame in pybullet handles = [] if has_gui() and view_ikfast: TCP_pb_pose = get_TCP_pose(pb_robot, ee_link_name, tcp_tf, return_pb_pose=True) handles = draw_pose(TCP_pb_pose, length=0.04) # wait_for_user() # deliver ros collision meshes to pybullet so_lists_from_name = convert_meshes_and_poses_to_pybullet_bodies(co_dict) static_obstacles_from_name = {} for so_key, so_val in so_lists_from_name.items(): for so_i, so_item in enumerate(so_val): static_obstacles_from_name[so_key + '_' + str(so_i)] = so_item for unit_name, unit_geo in unit_geos.items(): geo_bodies = [] for sub_id, mesh in enumerate(unit_geo.mesh): geo_bodies.append(convert_mesh_to_pybullet_body(mesh)) unit_geo.pybullet_bodies = geo_bodies # check collision between obstacles and element geometries assert not sanity_check_collisions(unit_geos, static_obstacles_from_name) # from random import shuffle seq_assignment = customized_sequence or list(range(len(unit_geos))) element_seq = {seq_id: e_id for seq_id, e_id in enumerate(seq_assignment)} to_seq_id = to_seq_id or len(element_seq) - 1 assert 0 <= from_seq_id and from_seq_id < len(element_seq) assert from_seq_id <= to_seq_id and to_seq_id < len(element_seq) if has_gui(): for e_id in element_seq.values(): handles.extend( draw_pose(unit_geos[e_id].initial_pb_pose, length=0.02)) handles.extend(draw_pose(unit_geos[e_id].goal_pb_pose, length=0.02)) for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) print('pybullet env loaded.') # wait_for_user() for h in handles: remove_debug(h) saved_world = WorldSaver() ik_fn = ikfast_ur3.get_ik if robot_model == 'ur3' else ikfast_ur5.get_ik tot_traj, graph_sizes = \ direct_ladder_graph_solve_picknplace(pb_robot, ik_joint_names, base_link_name, ee_link_name, ik_fn, unit_geos, element_seq, static_obstacles_from_name, from_seq_id=from_seq_id, to_seq_id=to_seq_id, pick_from_same_rack=pick_from_same_rack, tcp_transf=pb_pose_from_Transformation(tcp_tf), ee_attachs=ee_attachs, disabled_collision_link_names=disabled_link_names, viz=view_ikfast, st_conf=robot_start_conf, num_cart_steps=num_cart_steps) picknplace_cart_plans = divide_nested_list_chunks(tot_traj, graph_sizes) saved_world.restore() print('Cartesian planning finished.') # reset robot and parts for better visualization set_joint_positions(pb_robot, pb_ik_joints, robot_start_conf) for ee in ee_attachs: ee.assign() for e_id in element_seq.values(): for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) def flatten_unit_geos_bodies(in_dict): out_list = [] for ug in in_dict.values(): out_list.extend(ug.pybullet_bodies) return out_list if plan_transit: print('Transition planning started.') disabled_collision_links = [(link_from_name(pb_robot, pair[0]), link_from_name(pb_robot, pair[1])) \ for pair in disabled_link_names] for seq_id in range(0, from_seq_id): e_id = element_seq[seq_id] for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].goal_pb_pose) for seq_id in range(from_seq_id, to_seq_id + 1): e_id = element_seq[seq_id] print('----\ntransition seq#{} element #{}'.format(seq_id, e_id)) if seq_id != from_seq_id: tr_start_conf = picknplace_cart_plans[ seq_id - 1 - from_seq_id]['place_retreat'][-1] else: tr_start_conf = robot_start_conf place2pick_st_conf = list(tr_start_conf) assert picknplace_cart_plans[seq_id - from_seq_id][ 'pick_approach'], 'pick approach not found in sequence {} (element id: {})!'.format( seq_id, e_id) place2pick_goal_conf = list( picknplace_cart_plans[seq_id - from_seq_id]['pick_approach'][0]) saved_world = WorldSaver() set_joint_positions(pb_robot, pb_ik_joints, place2pick_st_conf) for ee_a in ee_attachs: ee_a.assign() built_obstacles = [] ignored_pairs = [] if pick_from_same_rack: built_obstacles = flatten_unit_geos_bodies({element_seq[prev_seq_id] : \ unit_geos[element_seq[prev_seq_id]] for prev_seq_id in range(seq_id)}) # if seq_id > 0: # ignored_pairs = list(product([ee_attach.child for ee_attach in ee_attachs], unit_geos[element_seq[seq_id-1]].pybullet_bodies)) else: built_obstacles = flatten_unit_geos_bodies(unit_geos) place2pick_obstacles = list( static_obstacles_from_name.values()) + built_obstacles place2pick_path = plan_joint_motion( pb_robot, pb_ik_joints, place2pick_goal_conf, attachments=ee_attachs, obstacles=place2pick_obstacles, disabled_collisions=disabled_collision_links, self_collisions=True, resolutions=[transit_res] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ignored_pairs=ignored_pairs) saved_world.restore() if not place2pick_path: saved_world = WorldSaver() print('****\nseq #{} cannot find place2pick transition'.format( seq_id)) print('Diagnosis...') cfn = get_collision_fn(pb_robot, pb_ik_joints, \ obstacles=place2pick_obstacles, attachments=ee_attachs, self_collisions=True, diagnosis=True) print('start pose:') cfn(place2pick_st_conf) print('end pose:') cfn(place2pick_goal_conf) saved_world.restore() print('Diagnosis over') assert picknplace_cart_plans[seq_id - from_seq_id][ 'pick_retreat'], 'pick retreat not found! in sequence {} (element id: {})!'.format( seq_id, e_id) assert picknplace_cart_plans[seq_id - from_seq_id][ 'place_approach'], 'place approach not found! in sequence {} (element id: {})!'.format( seq_id, e_id) pick2place_st_conf = picknplace_cart_plans[ seq_id - from_seq_id]['pick_retreat'][-1] pick2place_goal_conf = picknplace_cart_plans[ seq_id - from_seq_id]['place_approach'][0] saved_world = WorldSaver() # create attachement without needing to keep track of grasp... set_joint_positions( pb_robot, pb_ik_joints, picknplace_cart_plans[seq_id - from_seq_id]['pick_retreat'][0]) # attachs = [Attachment(robot, tool_link, invert(grasp.attach), e_body) for e_body in brick.body] element_attachs = [create_attachment(pb_robot, pb_end_effector_link, e_body) \ for e_body in unit_geos[e_id].pybullet_bodies] set_joint_positions(pb_robot, pb_ik_joints, pick2place_st_conf) for ee_a in ee_attachs: ee_a.assign() for e_a in element_attachs: e_a.assign() built_obstacles = [] if pick_from_same_rack: built_obstacles = flatten_unit_geos_bodies({element_seq[prev_seq_id] : \ unit_geos[element_seq[prev_seq_id]] for prev_seq_id in range(seq_id)}) else: built_obstacles = flatten_unit_geos_bodies(unit_geos) pick2place_obstacles = list( static_obstacles_from_name.values()) + built_obstacles pick2place_path = plan_joint_motion( pb_robot, pb_ik_joints, pick2place_goal_conf, disabled_collisions=disabled_collision_links, obstacles=pick2place_obstacles, attachments=ee_attachs + element_attachs, self_collisions=True, resolutions=[transit_res] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() if not pick2place_path: saved_world = WorldSaver() print('****\nseq #{} cannot find pick2place transition'.format( seq_id)) print('Diagnosis...') cfn = get_collision_fn(pb_robot, pb_ik_joints, obstacles=pick2place_obstacles, \ attachments=ee_attachs + element_attachs, self_collisions=True, diagnosis=True) print('start pose:') cfn(pick2place_st_conf) print('end pose:') cfn(pick2place_goal_conf) saved_world.restore() print('Diagnosis over') picknplace_cart_plans[seq_id - from_seq_id]['place2pick'] = place2pick_path picknplace_cart_plans[seq_id - from_seq_id]['pick2place'] = pick2place_path for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].goal_pb_pose) if seq_id == to_seq_id: saved_world = WorldSaver() return2idle_st_conf = picknplace_cart_plans[ seq_id - from_seq_id]['place_retreat'][-1] return2idle_goal_conf = robot_start_conf set_joint_positions(pb_robot, pb_ik_joints, return2idle_st_conf) for ee_a in ee_attachs: ee_a.assign() built_obstacles = flatten_unit_geos_bodies({element_seq[prev_seq_id] : \ unit_geos[element_seq[prev_seq_id]] for prev_seq_id in range(seq_id+1)}) return2idle_obstacles = list( static_obstacles_from_name.values()) + built_obstacles return2idle_path = plan_joint_motion( pb_robot, pb_ik_joints, return2idle_goal_conf, disabled_collisions=disabled_collision_links, obstacles=return2idle_obstacles, attachments=ee_attachs, self_collisions=True, resolutions=[transit_res] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) if not return2idle_path: saved_world = WorldSaver() print('****\nseq #{} cannot find return2idle transition'. format(seq_id)) print('Diagnosis...') cfn = get_collision_fn(pb_robot, pb_ik_joints, \ obstacles=return2idle_obstacles, attachments=ee_attachs, self_collisions=True, diagnosis=True) print('start pose:') cfn(return2idle_st_conf) print('end pose:') cfn(return2idle_goal_conf) saved_world.restore() print('Diagnosis over') saved_world.restore() picknplace_cart_plans[ seq_id - from_seq_id]['return2idle'] = return2idle_path print('Transition planning finished.') # convert to ros JointTrajectory traj_json_data = [] traj_time_count = 0.0 for i, element_process in enumerate(picknplace_cart_plans): e_proc_data = {} for sub_proc_name, sub_process in element_process.items(): sub_process_jt_traj_list = [] if not sub_process: continue for jt_sol in sub_process: sub_process_jt_traj_list.append( JointTrajectoryPoint(values=jt_sol, types=[0] * 6, time_from_start=Duration( traj_time_count, 0))) traj_time_count += 1.0 # meaningless timestamp e_proc_data[sub_proc_name] = JointTrajectory( trajectory_points=sub_process_jt_traj_list, start_configuration=sub_process_jt_traj_list[0]).to_data() traj_json_data.append(e_proc_data) if result_save_path: if not os.path.exists(os.path.dirname(result_save_path)): os.mkdir(os.path.dirname(result_save_path)) with open(result_save_path, 'w+') as outfile: json.dump(traj_json_data, outfile) print('planned trajectories saved to {}'.format(result_save_path)) print('\n*************************\nplanning completed.') if sim_traj and has_gui(): # if has_gui(): # wait_for_user() for e_id in element_seq.values(): for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) display_picknplace_trajectories(pb_robot, ik_joint_names, ee_link_name, unit_geos, traj_json_data, \ element_seq=element_seq, from_seq_id=from_seq_id, to_seq_id=to_seq_id, ee_attachs=ee_attachs, cartesian_time_step=cart_ts, transition_time_step=trans_ts, step_sim=step_sim, per_conf_step=per_conf_step) return traj_json_data
def main(): parser = argparse.ArgumentParser() # ur_picknplace_multiple_piece parser.add_argument('-p', '--problem', default='ur_picknplace_single_piece', help='The name of the problem to solve') parser.add_argument('-rob', '--robot', default='ur3', help='The type of UR robot to use.') parser.add_argument('-m', '--plan_transit', action='store_false', help='Plans motions between each picking and placing') parser.add_argument('-v', '--viewer', action='store_true', help='Enables the viewer during planning (slow!)') parser.add_argument('-s', '--save_result', action='store_true', help='save planning results as a json file') parser.add_argument( '-scale', '--model_scale', default=0.001, help='model scale conversion to meter, default 0.001 (from millimeter)' ) parser.add_argument('-vik', '--view_ikfast', action='store_true', help='Visualize each ikfast solutions') parser.add_argument('-tres', '--transit_res', default=0.01, help='joint resolution (rad)') parser.add_argument('-ros', '--use_ros', action='store_true', help='use ros backend with moveit planners') parser.add_argument('-cart_ts', '--cartesian_time_step', default=0.1, help='cartesian time step in trajectory simulation') parser.add_argument('-trans_ts', '--transit_time_step', default=0.01, help='transition time step in trajectory simulation') parser.add_argument('-per_conf_step', '--per_conf_step', action='store_true', help='stepping each configuration in simulation') args = parser.parse_args() print('Arguments:', args) VIZ = args.viewer VIZ_IKFAST = args.view_ikfast TRANSITION_JT_RESOLUTION = float(args.transit_res) plan_transition = args.plan_transit use_moveit_planner = args.use_ros # sim settings CART_TIME_STEP = args.cartesian_time_step TRANSITION_TIME_STEP = args.transit_time_step PER_CONF_STEP = args.per_conf_step # transition motion planner settings RRT_RESTARTS = 5 RRT_ITERATIONS = 40 # choreo pkg settings choreo_problem_instance_dir = compas_fab.get('choreo_instances') unit_geos, static_obstacles = load_assembly_package( choreo_problem_instance_dir, args.problem, scale=args.model_scale) result_save_path = os.path.join( choreo_problem_instance_dir, 'results', 'choreo_result.json') if args.save_result else None # urdf, end effector settings if args.robot == 'ur3': # urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur3.urdf') urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur3_collision_viz.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur3_moveit_config/config/ur3.srdf') else: urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03.obj') # ee_sep_filename = compas_fab.get('universal_robot/ur_description/meshes/' + # 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03_rough_sep.obj') # ee_decomp_file_dir = compas_fab.get('universal_robot/ur_description/meshes/' + # 'pychoreo_workshop_gripper/collision/decomp') # ee_decomp_file_prefix = 'victor_gripper_jaw03_decomp_' # decomp_parts_num = 36 client = RosClient() if use_moveit_planner else None # geometry file is not loaded here model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics, client=client) group = robot.main_group_name base_link_name = robot.get_base_link_name() ee_link_name = robot.get_end_effector_link_name() ik_joint_names = robot.get_configurable_joint_names() # parse end effector mesh # ee_meshes = [Mesh.from_obj(os.path.join(ee_decomp_file_dir, ee_decomp_file_prefix + str(i) + '.obj')) for i in range(decomp_parts_num)] ee_meshes = [Mesh.from_obj(ee_filename)] # ee_meshes = [Mesh.from_obj(ee_sep_filename)] # define TCP transformation tcp_tf = Translation([0.099, 0, 0]) # in meters ur5_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0] if use_moveit_planner: # TODO: attach end effector to the robot in planning scene # https://github.com/compas-dev/compas_fab/issues/66 scene = PlanningScene(robot) scene.remove_all_collision_objects() client.set_joint_positions(group, ik_joint_names, ur5_start_conf) else: scene = None # add static collision obstacles co_dict = {} for i, static_obs_mesh in enumerate(static_obstacles): # offset the table a bit... cm = CollisionMesh(static_obs_mesh, 'so_' + str(i), frame=Frame.from_transformation( Translation([0, 0, -0.02]))) if use_moveit_planner: scene.add_collision_mesh(cm) else: co_dict[cm.id] = {} co_dict[cm.id]['meshes'] = [cm.mesh] co_dict[cm.id]['mesh_poses'] = [cm.frame] if use_moveit_planner: # See: https://github.com/compas-dev/compas_fab/issues/63#issuecomment-519525879 time.sleep(1) co_dict = scene.get_collision_meshes_and_poses() # ====================================================== # ====================================================== # start pybullet environment & load pybullet robot connect(use_gui=VIZ) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, planning_scene=scene, ee_link_name=ee_link_name) ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) # update current joint conf and attach end effector pb_ik_joints = joints_from_names(pb_robot, ik_joint_names) pb_end_effector_link = link_from_name(pb_robot, ee_link_name) if not use_moveit_planner: set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf) for e_at in ee_attachs: e_at.assign() # draw TCP frame in pybullet if has_gui(): TCP_pb_pose = get_TCP_pose(pb_robot, ee_link_name, tcp_tf, return_pb_pose=True) handles = draw_pose(TCP_pb_pose, length=0.04) # wait_for_user() # deliver ros collision meshes to pybullet static_obstacles_from_name = convert_meshes_and_poses_to_pybullet_bodies( co_dict) # for now... for so_key, so_val in static_obstacles_from_name.items(): static_obstacles_from_name[so_key] = so_val[0] for unit_name, unit_geo in unit_geos.items(): geo_bodies = [] for sub_id, mesh in enumerate(unit_geo.mesh): geo_bodies.append(convert_mesh_to_pybullet_body(mesh)) unit_geo.pybullet_bodies = geo_bodies # check collision between obstacles and element geometries assert not sanity_check_collisions(unit_geos, static_obstacles_from_name) # from random import shuffle seq_assignment = list(range(len(unit_geos))) # shuffle(seq_assignment) element_seq = {seq_id: e_id for seq_id, e_id in enumerate(seq_assignment)} # for key, val in element_seq.items(): # # element_seq[key] = 'e_' + str(val) # element_seq[key] = val if has_gui(): for e_id in element_seq.values(): # for e_body in brick_from_index[e_id].body: set_pose(e_body, brick_from_index[e_id].goal_pose) handles.extend( draw_pose(unit_geos[e_id].initial_pb_pose, length=0.02)) handles.extend(draw_pose(unit_geos[e_id].goal_pb_pose, length=0.02)) for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) print('pybullet env loaded.') # wait_for_user() for h in handles: remove_debug(h) saved_world = WorldSaver() ik_fn = ikfast_ur3.get_ik if args.robot == 'ur3' else ikfast_ur5.get_ik tot_traj, graph_sizes = \ direct_ladder_graph_solve_picknplace(pb_robot, ik_joint_names, base_link_name, ee_link_name, ik_fn, unit_geos, element_seq, static_obstacles_from_name, tcp_transf=pb_pose_from_Transformation(tcp_tf), ee_attachs=ee_attachs, max_attempts=100, viz=VIZ_IKFAST, st_conf=ur5_start_conf) picknplace_cart_plans = divide_nested_list_chunks(tot_traj, graph_sizes) saved_world.restore() print('Cartesian planning finished.') # reset robot and parts for better visualization set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf) for ee in ee_attachs: ee.assign() for e_id in element_seq.values(): for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) # if has_gui(): # wait_for_user() def flatten_unit_geos_bodies(in_dict): out_list = [] for ug in in_dict.values(): out_list.extend(ug.pybullet_bodies) return out_list if plan_transition: print('Transition planning started.') for seq_id, unit_picknplace in enumerate(picknplace_cart_plans): print('----\ntransition seq#{}'.format(seq_id)) e_id = element_seq[seq_id] if seq_id != 0: tr_start_conf = picknplace_cart_plans[seq_id - 1]['place_retreat'][-1] else: tr_start_conf = ur5_start_conf # obstacles=static_obstacles + cur_mo_list place2pick_st_conf = list(tr_start_conf) place2pick_goal_conf = list( picknplace_cart_plans[seq_id]['pick_approach'][0]) # assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_st_conf) # assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_goal_conf) if use_moveit_planner: # TODO: add collision objects st_conf = Configuration.from_revolute_values( place2pick_st_conf) goal_conf = Configuration.from_revolute_values( place2pick_goal_conf) goal_constraints = robot.constraints_from_configuration( goal_conf, [math.radians(1)] * 6, group) place2pick_jt_traj = robot.plan_motion(goal_constraints, st_conf, group, planner_id='RRTConnect') place2pick_path = [ jt_pt['values'] for jt_pt in place2pick_jt_traj.to_data()['points'] ] else: saved_world = WorldSaver() set_joint_positions(pb_robot, pb_ik_joints, place2pick_st_conf) for ee_a in ee_attachs: ee_a.assign() place2pick_path = plan_joint_motion( pb_robot, pb_ik_joints, place2pick_goal_conf, attachments=ee_attachs, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), self_collisions=True, resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() if not place2pick_path: saved_world = WorldSaver() print('****\nseq #{} cannot find place2pick transition'. format(seq_id)) print('Diagnosis...') cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints, \ obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), attachments=ee_attachs, self_collisions=True) print('start pose:') cfn(place2pick_st_conf) print('end pose:') cfn(place2pick_goal_conf) saved_world.restore() print('Diagnosis over') pick2place_st_conf = picknplace_cart_plans[seq_id]['pick_retreat'][ -1] pick2place_goal_conf = picknplace_cart_plans[seq_id][ 'place_approach'][0] if use_moveit_planner: st_conf = Configuration.from_revolute_values( picknplace_cart_plans[seq_id]['pick_retreat'][-1]) goal_conf = Configuration.from_revolute_values( picknplace_cart_plans[seq_id]['place_approach'][0]) goal_constraints = robot.constraints_from_configuration( goal_conf, [math.radians(1)] * 6, group) pick2place_jt_traj = robot.plan_motion(goal_constraints, st_conf, group, planner_id='RRTConnect') pick2place_path = [ jt_pt['values'] for jt_pt in pick2place_jt_traj.to_data()['points'] ] else: saved_world = WorldSaver() # create attachement without needing to keep track of grasp... set_joint_positions( pb_robot, pb_ik_joints, picknplace_cart_plans[seq_id]['pick_retreat'][0]) # attachs = [Attachment(robot, tool_link, invert(grasp.attach), e_body) for e_body in brick.body] element_attachs = [create_attachment(pb_robot, pb_end_effector_link, e_body) \ for e_body in unit_geos[e_id].pybullet_bodies] set_joint_positions(pb_robot, pb_ik_joints, pick2place_st_conf) for ee_a in ee_attachs: ee_a.assign() for e_a in element_attachs: e_a.assign() pick2place_path = plan_joint_motion( pb_robot, pb_ik_joints, pick2place_goal_conf, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), attachments=ee_attachs + element_attachs, self_collisions=True, resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() if not pick2place_path: saved_world = WorldSaver() print('****\nseq #{} cannot find pick2place transition'. format(seq_id)) print('Diagnosis...') cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), \ attachments=ee_attachs + element_attachs, self_collisions=True) print('start pose:') cfn(pick2place_st_conf) print('end pose:') cfn(pick2place_goal_conf) saved_world.restore() print('Diagnosis over') picknplace_cart_plans[seq_id]['place2pick'] = place2pick_path picknplace_cart_plans[seq_id]['pick2place'] = pick2place_path for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].goal_pb_pose) if seq_id == len(picknplace_cart_plans) - 1: saved_world = WorldSaver() set_joint_positions( pb_robot, pb_ik_joints, picknplace_cart_plans[seq_id]['place_retreat'][-1]) for ee_a in ee_attachs: ee_a.assign() return2idle_path = plan_joint_motion( pb_robot, pb_ik_joints, ur5_start_conf, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), attachments=ee_attachs, self_collisions=True, resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() picknplace_cart_plans[seq_id]['return2idle'] = return2idle_path print('Transition planning finished.') # convert to ros JointTrajectory traj_json_data = [] traj_time_count = 0.0 for i, element_process in enumerate(picknplace_cart_plans): e_proc_data = {} for sub_proc_name, sub_process in element_process.items(): sub_process_jt_traj_list = [] for jt_sol in sub_process: sub_process_jt_traj_list.append( JointTrajectoryPoint(values=jt_sol, types=[0] * 6, time_from_start=Duration( traj_time_count, 0))) traj_time_count += 1.0 # meaningless timestamp e_proc_data[sub_proc_name] = JointTrajectory( trajectory_points=sub_process_jt_traj_list, start_configuration=sub_process_jt_traj_list[0]).to_data() traj_json_data.append(e_proc_data) if result_save_path: with open(result_save_path, 'w+') as outfile: json.dump(traj_json_data, outfile, indent=4) print('planned trajectories saved to {}'.format(result_save_path)) print('\n*************************\nplanning completed. Simulate?') if has_gui(): wait_for_user() for e_id in element_seq.values(): for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) display_picknplace_trajectories(pb_robot, ik_joint_names, ee_link_name, unit_geos, traj_json_data, \ ee_attachs=ee_attachs, cartesian_time_step=CART_TIME_STEP, transition_time_step=TRANSITION_TIME_STEP, step_sim=True, per_conf_step=PER_CONF_STEP) if use_moveit_planner: scene.remove_all_collision_objects()
DATA = os.path.join(HERE, '../data') PATH = os.path.join(DATA, 'robot_description') #package = 'ur_description' package = 'ur_setups' urdf_filename = os.path.join(PATH, package, "urdf", "ur10_with_measurement_tool.urdf") srdf_filename = os.path.join(PATH, package, "ur10_with_measurement_tool.srdf") #package = "abb_linear_axis" #urdf_filename = os.path.join(PATH, package, "urdf", "abb_linear_axis_brick_suction_tool.urdf") #srdf_filename = os.path.join(PATH, package, "abb_linear_axis_suction_tool.srdf") model = RobotModel.from_urdf_file(urdf_filename) loaders = [] loaders.append(LocalPackageMeshLoader(PATH, "ur_description")) loaders.append(LocalPackageMeshLoader(PATH, "ur_end_effectors")) #loaders.append(LocalPackageMeshLoader(PATH, "abb_linear_axis")) #loaders.append(LocalPackageMeshLoader(PATH, "abb_irb4600_40_255")) #loaders.append(LocalPackageMeshLoader(PATH, "abb_end_effectors")) model.load_geometry(*loaders) artist = None # artist = RobotArtist(model) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = Robot(model, artist, semantics, client=None) robot.info()
def single_place_check(seq_id, assembly_json_path, customized_sequence=[], num_cart_steps=10, robot_model='ur3', enable_viewer=True, view_ikfast=False, tcp_tf_list=[1e-3 * 80.525, 0, 0], scale=1, diagnosis=False, viz_time_gap=0.5): # # rescaling # # TODO: this should be done when the Assembly object is made unit_geos, static_obstacle_meshes = load_assembly_package( assembly_json_path, scale=scale) assert seq_id < len(unit_geos) if customized_sequence: unit_geo = unit_geos[customized_sequence[seq_id]] else: unit_geo = unit_geos[seq_id] # urdf, end effector settings if robot_model == 'ur3': urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur3.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur3_moveit_config/config/ur3.srdf') else: urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'dms_2019_gripper/collision/190907_Gripper_05.obj') # geometry file is not loaded here model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics) base_link_name = robot.get_base_link_name() ee_link_name = robot.get_end_effector_link_name() ik_joint_names = robot.get_configurable_joint_names() disabled_link_names = semantics.get_disabled_collisions() # parse end effector mesh ee_meshes = [Mesh.from_obj(ee_filename)] tcp_tf = Translation(tcp_tf_list) # ====================================================== # ====================================================== # start pybullet environment & load pybullet robot connect(use_gui=enable_viewer) camera_base_pt = (0, 0, 0) camera_pt = np.array(camera_base_pt) + np.array([1, 0, 0.5]) set_camera_pose(tuple(camera_pt), camera_base_pt) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, ee_link_name=ee_link_name) ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) # update current joint conf and attach end effector pb_ik_joints = joints_from_names(pb_robot, ik_joint_names) pb_end_effector_link = link_from_name(pb_robot, ee_link_name) robot_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0] set_joint_positions(pb_robot, pb_ik_joints, robot_start_conf) for e_at in ee_attachs: e_at.assign() # viz handles handles = [] # convert mesh into pybullet bodies # TODO: this conversion should be moved into UnitGeometry geo_bodies = [] for mesh in unit_geo.mesh: geo_bodies.append(convert_mesh_to_pybullet_body(mesh)) unit_geo.pybullet_bodies = geo_bodies static_obstacles = [] for so_mesh in static_obstacle_meshes: static_obstacles.append(convert_mesh_to_pybullet_body(so_mesh)) if customized_sequence: assembled_element_ids = [ customized_sequence[seq_iter] for seq_iter in range(seq_id) ] else: assembled_element_ids = list(range(seq_id)) print('assembled_ids: ', assembled_element_ids) for prev_e_id in assembled_element_ids: other_unit_geo = unit_geos[prev_e_id] print('other unit geo: ', other_unit_geo) for _, mesh in enumerate(other_unit_geo.mesh): pb_e = convert_mesh_to_pybullet_body(mesh) set_pose(pb_e, other_unit_geo.goal_pb_pose) static_obstacles.append(pb_e) # check collision between obstacles and element geometries # assert not sanity_check_collisions([unit_geo], static_obstacles_from_name) ik_fn = ikfast_ur3.get_ik if robot_model == 'ur3' else ikfast_ur5.get_ik return quick_check_place_feasibility( pb_robot, ik_joint_names, base_link_name, ee_link_name, ik_fn, unit_geo, num_cart_steps=num_cart_steps, static_obstacles=static_obstacles, self_collisions=True, mount_link_from_tcp_pose=pb_pose_from_Transformation(tcp_tf), ee_attachs=ee_attachs, viz=view_ikfast, disabled_collision_link_names=disabled_link_names, diagnosis=diagnosis, viz_time_gap=viz_time_gap)
def viz_saved_traj(traj_save_path, assembly_json_path, element_seq, from_seq_id, to_seq_id, cart_ts=0.01, trans_ts=0.01, scale=0.001, robot_model='ur3', per_conf_step=False, step_sim=False): assert os.path.exists(traj_save_path) and os.path.exists( assembly_json_path) # rescaling # TODO: this should be done when the Assembly object is made unit_geos, static_obstacle_meshes = load_assembly_package( assembly_json_path, scale=scale) # urdf, end effector settings if robot_model == 'ur3': urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur3.urdf') # urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur3_collision_viz.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur3_moveit_config/config/ur3.srdf') else: urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'dms_2019_gripper/collision/190907_Gripper_05.obj') model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics) ik_joint_names = robot.get_configurable_joint_names() ee_link_name = robot.get_end_effector_link_name() connect(use_gui=True) camera_base_pt = (0, 0, 0) camera_pt = np.array(camera_base_pt) + np.array([1, 0, 0.5]) set_camera_pose(tuple(camera_pt), camera_base_pt) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, ee_link_name=ee_link_name) ee_meshes = [Mesh.from_obj(ee_filename)] ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) static_obstacles = [] for so_mesh in static_obstacle_meshes: static_obstacles.append(convert_mesh_to_pybullet_body(so_mesh)) for unit_name, unit_geo in unit_geos.items(): geo_bodies = [] for sub_id, mesh in enumerate(unit_geo.mesh): geo_bodies.append(convert_mesh_to_pybullet_body(mesh)) unit_geo.pybullet_bodies = geo_bodies if os.path.exists(traj_save_path): with open(traj_save_path, 'r') as f: traj_json_data = json.loads(f.read()) for e_id in element_seq: for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) display_picknplace_trajectories(pb_robot, ik_joint_names, ee_link_name, unit_geos, traj_json_data, \ element_seq=element_seq, from_seq_id=from_seq_id, to_seq_id=to_seq_id, ee_attachs=ee_attachs, cartesian_time_step=cart_ts, transition_time_step=trans_ts, step_sim=step_sim, per_conf_step=per_conf_step) else: print('no saved traj found at: ', traj_save_path)
def panda_robot_instance(panda_urdf, panda_srdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) return Robot(model, semantics=semantics)
def test_ikfast_inverse_kinematics(): urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get('universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics) base_link_name = robot.get_base_link_name() ik_joint_names = robot.get_configurable_joint_names() ik_tool_link_name = robot.get_end_effector_link_name() connect(use_gui=False) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name) pb_ik_joints = joints_from_names(pb_robot, ik_joint_names) max_attempts = 20 EPS = 1e-3 sample_fn = get_sample_fn(pb_robot, pb_ik_joints) for i in range(max_attempts): # randomly sample within joint limits conf = sample_fn() # sanity joint limit violation check assert not violates_limits(pb_robot, pb_ik_joints, conf) # ikfast's FK fk_fn = ikfast_ur5.get_fk ikfast_FK_pb_pose = get_ik_tool_link_pose(fk_fn, pb_robot, ik_joint_names, base_link_name, conf) if has_gui(): print('test round #{}: ground truth conf: {}'.format(i, conf)) handles = draw_pose(ikfast_FK_pb_pose, length=0.04) set_joint_positions(pb_robot, pb_ik_joints, conf) wait_for_user() # ikfast's IK ik_fn = ikfast_ur5.get_ik ik_sols = sample_tool_ik(ik_fn, pb_robot, ik_joint_names, base_link_name, ikfast_FK_pb_pose, get_all=True) # TODO: UR robot or in general joint w/ domain over 4 pi # needs specialized distance function q_selected = sample_tool_ik(ik_fn, pb_robot, ik_joint_names, base_link_name, ikfast_FK_pb_pose, nearby_conf=True) qsol = best_sol(ik_sols, conf, [1.]*6) print('q selected: {}'.format(q_selected)) print('q best: {}'.format(qsol)) if has_gui(): set_joint_positions(pb_robot, pb_ik_joints, qsol) wait_for_user() for h in handles : remove_debug(h) if qsol is None: qsol = [999.]*6 diff = np.sum(np.abs(np.array(qsol) - np.array(conf))) if diff > EPS: print(np.array(ik_sols)) print('Best q:{}'.format(qsol)) print('Actual:{}'.format(np.array(conf))) print('Diff: {}'.format(conf - qsol)) print('Difdiv:{}'.format((conf - qsol)/np.pi)) assert False