def from_t0cf_to_tcf(self, frames_t0cf): """Converts frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame). Parameters ---------- frames_t0cf : list[:class:`compas.geometry.Frame`] Frames (in WCF) at the robot's flange (tool0). Returns ------- list[:class:`compas.geometry.Frame`] Frames (in WCF) at the robot's tool tip (tcf). Examples -------- >>> import compas >>> from compas.datastructures import Mesh >>> from compas.geometry import Frame >>> mesh = Mesh.from_stl(compas.get('cone.stl')) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = ToolModel(mesh, frame) >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] >>> tool.from_t0cf_to_tcf(frames_t0cf) [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] """ Te = Transformation.from_frame_to_frame(Frame.worldXY(), self.frame) return [ Frame.from_transformation(Transformation.from_frame(f) * Te) for f in frames_t0cf ]
def convert_frame_wcf_to_frame_tool0_rcf(self, frame_wcf, base_transformation=None, tool_transformation=None): T = Transformation.from_frame(frame_wcf) if base_transformation: T = base_transformation * T if tool_transformation: T = T * tool_transformation return Frame.from_transformation(T)
def frame(self) -> compas.geometry.Frame: location = self.shape.Location() transformation = location.Transformation() T = Transformation( matrix=[[transformation.Value(i, j) for j in range(4)] for i in range(4)]) frame = Frame.from_transformation(T) return frame
def get_TCP_pose_from_joint_values(joint_values, robot_model='ur3', tcp_tf_list=[1e-3 * 80.525, 0, 0]): if robot_model == 'ur3': fk_fn = ikfast_ur3.get_fk elif robot_model == 'ur5': fk_fn = ikfast_ur5.get_fk else: raise ValueError('Not supported robot model!') pb_pose = compute_forward_kinematics(fk_fn, joint_values) mount_frame = Frame_from_pb_pose(pb_pose) if tcp_tf_list: world_from_mount = Transformation.from_frame(mount_frame) mount_from_TCP = Translation(tcp_tf_list) TCP_frame = Frame.from_transformation(\ Transformation.concatenate(world_from_mount, mount_from_TCP)) return TCP_frame.to_data() else: return mount_frame.to_data()
def get_transformed_tool_frames(self, T5): T = self.get_tool0_transformation(T5) tool0_frame = Frame.from_transformation(T) tcp_frame = Frame.from_transformation(T * self.transformation_tcp_tool0) return tool0_frame, tcp_frame
def get_tcp_frame_from_tool0_frame(self, frame_tool0): """Get the tcp frame from the tool0 frame. """ T = Transformation.from_frame(frame_tool0) return Frame.from_transformation(T * self.transformation_tcp_tool0)
def get_tool0_frame_from_tcp_frame(self, frame_tcp): """Get the tool0 frame (frame at robot) from the tool frame (frame_tcp). """ T = Transformation.from_frame(frame_tcp) return Frame.from_transformation(T * self.transformation_tool0_tcp)
def test_from_frame(): f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) T = Transformation.from_frame(f1) f2 = Frame.from_transformation(T) assert np.allclose(f1, f2)
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()
def inverse_kinematics(robot, frame_WCF, start_configuration=None, group=None, options=None): # TODO use input here instead of class attributes # avoid_collisions=True, constraints=None, # attempts=8, attached_collision_meshes=None, # return_full_configuration=False, # cull=False, # return_closest_to_start=False, # return_idxs=None): avoid_collisions = is_valid_option(options, 'avoid_collisions', True) constraints = is_valid_option(options, 'constraints', None) attempts = is_valid_option(options, 'attempts', 8) attached_collision_meshes = is_valid_option(options, 'attached_collision_meshes', None) return_full_configuration = is_valid_option(options, 'return_full_configuration', False) cull = is_valid_option(options, 'cull', False) return_closest_to_start = is_valid_option(options, 'return_closest_to_start', False) return_idxs = is_valid_option(options, 'return_idxs', None) if start_configuration: base_frame = robot.get_base_frame(self.group, full_configuration=start_configuration) self.update_base_transformation(base_frame) # in_collision = robot.client.configuration_in_collision(start_configuration) # if in_collision: # raise ValueError("Start configuration already in collision") # frame_tool0_RCF = self.convert_frame_wcf_to_frame_tool0_rcf(frame_WCF) frame_tool0_RCF = Frame.from_transformation(self.base_transformation * Transformation.from_frame(frame_WCF) * self.tool_transformation) # call the ik function configurations = self.function(frame_tool0_RCF) # The ik solution for 6 axes industrial robots returns by default 8 # configurations, which are sorted. That means, the if you call ik # on 2 frames that are close to each other, and compare the 8 # configurations of the first one with the 8 of the second one at # their respective indices, then these configurations are 'close' to # each other. That is why for certain use cases, e.g. custom cartesian # path planning it makes sense to keep the sorting and set the ones # that are out of joint limits or in collison to `None`. if return_idxs: configurations = [configurations[i] for i in return_idxs] # add joint names to configurations self.add_joint_names_to_configurations(configurations) # fit configurations within joint bounds (sets those to `None` that are not working) self.try_to_fit_configurations_between_bounds(configurations) # check collisions for all configurations (sets those to `None` that are not working) # TODO defer collision checking # if robot.client: # robot.client.check_configurations_for_collision(configurations) if return_closest_to_start: diffs = [c.max_difference(start_configuration) for c in configurations if c is not None] if len(diffs): idx = diffs.index(min(diffs)) return configurations[idx] # only one return None if cull: configurations = [c for c in configurations if c is not None] return configurations
def convert_frame_wcf_to_tool0_wcf(self, frame_wcf): return Frame.from_transformation(Transformation.from_frame(frame_wcf) * self.tool_transformation)
def test_collision_checker(abb_irb4600_40_255_setup, itj_TC_g1_cms, itj_beam_cm, column_obstacle_cm, base_plate_cm, itj_tool_changer_grasp_transf, itj_gripper_grasp_transf, itj_beam_grasp_transf, tool_type, itj_tool_changer_urdf_path, itj_g1_urdf_path, viewer, diagnosis): # modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py urdf_filename, semantics = abb_irb4600_40_255_setup move_group = 'bare_arm' ee_touched_link_names = ['link_6'] with PyChoreoClient(viewer=viewer) as client: with LockRenderer(): robot = client.load_robot(urdf_filename) robot.semantics = semantics client.disabled_collisions = robot.semantics.disabled_collisions if tool_type == 'static': for _, ee_cm in itj_TC_g1_cms.items(): client.add_collision_mesh(ee_cm) else: client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path) client.add_tool_from_urdf('g1', itj_g1_urdf_path) # * add static obstacles client.add_collision_mesh(base_plate_cm) client.add_collision_mesh(column_obstacle_cm) ik_joint_names = robot.get_configurable_joint_names(group=move_group) ik_joint_types = robot.get_joint_types_by_names(ik_joint_names) flange_link_name = robot.get_end_effector_link_name(group=move_group) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_tool_changer_base = itj_tool_changer_grasp_transf tool0_from_gripper_base = itj_gripper_grasp_transf client.set_object_frame( '^{}'.format('TC'), Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base)) client.set_object_frame( '^{}'.format('g1'), Frame.from_transformation(tool0_tf * tool0_from_gripper_base)) names = client._get_collision_object_names('^{}'.format('g1')) + \ client._get_collision_object_names('^{}'.format('TC')) for ee_name in names: attach_options = {'robot': robot} if tool_type == 'actuated': attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base' attach_options.update( {'attached_child_link_name': attached_child_link_name}) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, ee_name), flange_link_name, touch_links=ee_touched_link_names), options=attach_options) # client._print_object_summary() # wait_if_gui('EE attached.') if tool_type == 'actuated': # lower 0.0008 upper 0.01 tool_bodies = client._get_bodies('^{}'.format('itj_PG500')) tool_conf = Configuration( values=[0.01, 0.01], types=[Joint.PRISMATIC, Joint.PRISMATIC], joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r']) for b in tool_bodies: client._set_body_configuration(b, tool_conf) wait_if_gui('Open') tool_conf = Configuration( values=[0.0008, 0.0008], types=[Joint.PRISMATIC, Joint.PRISMATIC], joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r']) for b in tool_bodies: client._set_body_configuration(b, tool_conf) wait_if_gui('Close') cprint('safe start conf', 'green') conf = Configuration(values=[0.] * 6, types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf, options={'diagnosis': diagnosis}) cprint('joint over limit', 'cyan') conf = Configuration(values=[0., 0., 1.5, 0, 0, 0], types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('attached gripper-obstacle collision - column', 'cyan') vals = [ -0.33161255787892263, -0.43633231299858238, 0.43633231299858238, -1.0471975511965976, 0.087266462599716474, 0.0 ] # conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) # client.set_robot_configuration(robot, conf) # wait_if_gui() assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) #* attach beam client.add_collision_mesh(itj_beam_cm) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_beam_base = itj_beam_grasp_transf client.set_object_frame( '^{}$'.format('itj_beam_b2'), Frame.from_transformation(tool0_tf * tool0_from_beam_base)) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, 'itj_beam_b2'), flange_link_name, touch_links=[]), options={'robot': robot}) # wait_if_gui('beam attached.') cprint('attached beam-robot body self collision', 'cyan') vals = [ 0.73303828583761843, -0.59341194567807209, 0.54105206811824214, -0.17453292519943295, 1.064650843716541, 1.7278759594743862 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('attached beam-obstacle collision - column', 'cyan') vals = [ 0.087266462599716474, -0.19198621771937624, 0.20943951023931956, 0.069813170079773182, 1.2740903539558606, 0.069813170079773182 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('attached beam-obstacle collision - ground', 'cyan') vals = [ -0.017453292519943295, 0.6108652381980153, 0.20943951023931956, 1.7627825445142729, 1.2740903539558606, 0.069813170079773182 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('robot link-obstacle collision - column', 'cyan') vals = [ -0.41887902047863912, 0.20943951023931956, 0.20943951023931956, 1.7627825445142729, 1.2740903539558606, 0.069813170079773182 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('robot link-obstacle collision - ground', 'cyan') vals = [ 0.33161255787892263, 1.4660765716752369, 0.27925268031909273, 0.17453292519943295, 0.22689280275926285, 0.54105206811824214 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('Sweeping collision', 'cyan') vals = [ -0.12217304763960307, -0.73303828583761843, 0.83775804095727824, -2.4609142453120048, 1.2391837689159739, -0.85521133347722145 ] conf1 = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf1, options={'diagnosis': diagnosis}) # wait_if_gui() vals = [ -0.12217304763960307, -0.73303828583761843, 0.83775804095727824, -2.4958208303518914, -1.5533430342749532, -0.85521133347722145 ] conf2 = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf2, options={'diagnosis': diagnosis}) # wait_if_gui() assert client.check_sweeping_collisions(robot, conf1, conf2, options={ 'diagnosis': diagnosis, 'line_width': 3.0 }) wait_if_gui("Finished.")
def test_plan_motion(abb_irb4600_40_255_setup, itj_TC_g1_cms, itj_beam_cm, column_obstacle_cm, base_plate_cm, itj_tool_changer_grasp_transf, itj_gripper_grasp_transf, itj_beam_grasp_transf, tool_type, itj_tool_changer_urdf_path, itj_g1_urdf_path, viewer, diagnosis): # modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py urdf_filename, semantics = abb_irb4600_40_255_setup move_group = 'bare_arm' ee_touched_link_names = ['link_6'] with PyChoreoClient(viewer=viewer) as client: with LockRenderer(): robot = client.load_robot(urdf_filename) robot.semantics = semantics client.disabled_collisions = robot.semantics.disabled_collisions if tool_type == 'static': for _, ee_cm in itj_TC_g1_cms.items(): client.add_collision_mesh(ee_cm) else: client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path) client.add_tool_from_urdf('g1', itj_g1_urdf_path) # * add static obstacles client.add_collision_mesh(base_plate_cm) client.add_collision_mesh(column_obstacle_cm) ik_joint_names = robot.get_configurable_joint_names(group=move_group) ik_joint_types = robot.get_joint_types_by_names(ik_joint_names) flange_link_name = robot.get_end_effector_link_name(group=move_group) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_tool_changer_base = itj_tool_changer_grasp_transf tool0_from_gripper_base = itj_gripper_grasp_transf client.set_object_frame( '^{}'.format('TC'), Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base)) client.set_object_frame( '^{}'.format('g1'), Frame.from_transformation(tool0_tf * tool0_from_gripper_base)) names = client._get_collision_object_names('^{}'.format('g1')) + \ client._get_collision_object_names('^{}'.format('TC')) for ee_name in names: attach_options = {'robot': robot} if tool_type == 'actuated': attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base' attach_options.update( {'attached_child_link_name': attached_child_link_name}) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, ee_name), flange_link_name, touch_links=ee_touched_link_names), options=attach_options) #* attach beam client.add_collision_mesh(itj_beam_cm) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_beam_base = itj_beam_grasp_transf client.set_object_frame( '^{}$'.format('itj_beam_b2'), Frame.from_transformation(tool0_tf * tool0_from_beam_base)) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, 'itj_beam_b2'), flange_link_name, touch_links=[]), options={'robot': robot}) wait_if_gui('beam attached.') vals = [ -1.4660765716752369, -0.22689280275926285, 0.27925268031909273, 0.17453292519943295, 0.22689280275926285, -0.22689280275926285 ] start_conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) # client.set_robot_configuration(robot, start_conf) # wait_if_gui() # vals = [0.05235987755982989, -0.087266462599716474, -0.05235987755982989, 1.7104226669544429, 0.13962634015954636, -0.43633231299858238] vals = [ 0.034906585039886591, 0.68067840827778847, 0.15707963267948966, -0.89011791851710809, -0.034906585039886591, -2.2514747350726849 ] end_conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) # client.set_robot_configuration(robot, end_conf) # wait_if_gui() plan_options = {'diagnosis': True, 'resolutions': 0.05} goal_constraints = robot.constraints_from_configuration( end_conf, [0.01], [0.01], group=move_group) st_time = time.time() trajectory = client.plan_motion(robot, goal_constraints, start_configuration=start_conf, group=move_group, options=plan_options) print('Solving time: {}'.format(elapsed_time(st_time))) if trajectory is None: cprint('Client motion planner CANNOT find a plan!', 'red') # assert False, 'Client motion planner CANNOT find a plan!' # TODO warning else: cprint('Client motion planning find a plan!', 'green') wait_if_gui('Start sim.') time_step = 0.03 for traj_pt in trajectory.points: client.set_robot_configuration(robot, traj_pt) wait_for_duration(time_step) wait_if_gui("Finished.")
# ============================================================================== if __name__ == "__main__": from compas.geometry import Point from compas.geometry import Frame from compas.geometry import allclose from compas.geometry import matrix_from_translation from compas.geometry import matrix_from_scale_factors from compas.geometry import transform_points from numpy import asarray f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) T = Transformation.from_frame(f1) f2 = Frame.from_transformation(T) print(f1 == f2) f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) T = Transformation.from_frame(f) Tinv = T.inverse() I = Transformation() print(I == T * Tinv) f1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26]) f2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) T = Transformation.from_frame_to_frame(f1, f2) f1.transform(T) print(f1 == f2) f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
def pose_from_transformation(tf, scale=1.0): frame = Frame.from_transformation(tf) return pose_from_frame(frame, scale)