def get_robot_configuration(self, robot, group): robot_uid = robot.attributes['pybullet_uid'] joint_names = robot.get_configurable_joint_names(group=group) joints = joints_from_names(robot_uid, joint_names) joint_types = robot.get_joint_types_by_names(joint_names) joint_values = get_joint_positions(robot_uid, joints) return Configuration(values=joint_values, types=joint_types, joint_names=joint_names)
def _get_sweeping_collision_fn(self, robot, joint_names, options=None): robot_uid = robot.attributes['pybullet_uid'] avoid_collisions = options.get('avoid_collisions', True) self_collisions = options.get('self_collisions', True) distance_threshold = options.get('distance_threshold', 0.0) max_distance = options.get('max_distance', 0.0) debug = options.get('debug', False) line_width = options.get('line_width', 1) # * custom joint limits ik_joints = joints_from_names(robot_uid, joint_names) obstacles, attachments, extra_disabled_collisions = self.client._get_collision_checking_setup( options) sweeping_collision_fn = get_attachment_sweeping_collision_fn( robot_uid, ik_joints, obstacles=obstacles, attachments=attachments, width=line_width) return sweeping_collision_fn
def _get_collision_fn(self, robot, joint_names, options=None): """Returns a `pybullet_planning` collision_fn """ robot_uid = robot.attributes['pybullet_uid'] custom_limits = options.get('custom_limits', {}) avoid_collisions = options.get('avoid_collisions', True) self_collisions = options.get('self_collisions', True) distance_threshold = options.get('distance_threshold', 0.0) max_distance = options.get('max_distance', 0.0) debug = options.get('debug', False) # * custom joint limits ik_joints = joints_from_names(robot_uid, joint_names) pb_custom_limits = get_custom_limits( robot_uid, ik_joints, custom_limits={ joint_from_name(robot_uid, jn): lims for jn, lims in custom_limits.items() }) obstacles, attachments, extra_disabled_collisions = self.client._get_collision_checking_setup( options) collision_fn = get_collision_fn( robot_uid, ik_joints, obstacles=obstacles, attachments=attachments, self_collisions=avoid_collisions and self_collisions, disabled_collisions=self.client.get_self_collision_link_ids( robot), # get disabled self-collision links (srdf) extra_disabled_collisions=extra_disabled_collisions, custom_limits=pb_custom_limits, body_name_from_id=self.client._name_from_body_id, distance_threshold=distance_threshold, max_distance=max_distance) return collision_fn
def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None): """Calculates a motion path. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the motion path is being calculated. goal_constraints: list of :class:`compas_fab.robots.Constraint` The goal to be achieved, defined in a set of constraints. Constraints can be very specific, for example defining value domains for each joint, such that the goal configuration is included, or defining a volume in space, to which a specific robot link (e.g. the end-effector) is required to move to. start_configuration: :class:`compas_fab.robots.Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. group: str, optional The name of the group to plan for. options : dict, optional Dictionary containing kwargs for arguments specific to the client being queried. - ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions. Defaults to ``True``. - ``"max_step"``: (:obj:`float`, optional) The approximate distance between the calculated points. (Defined in the robot's units.) Defaults to ``0.01``. - ``"jump_threshold"``: (:obj:`float`, optional) The maximum allowed distance of joint positions between consecutive points. If the distance is found to be above this threshold, the path computation fails. It must be specified in relation to max_step. If this threshold is ``0``, 'jumps' might occur, resulting in an invalid cartesian path. Defaults to :math:`\\pi / 2`. Returns ------- :class:`compas_fab.robots.JointTrajectory` The calculated trajectory. """ robot_uid = robot.attributes['pybullet_uid'] # * parse options verbose = is_valid_option(options, 'verbose', False) diagnosis = options.get('diagnosis', False) custom_limits = options.get('custom_limits') or {} resolutions = options.get('resolutions') or 0.1 weights = options.get('weights') or None rrt_restarts = options.get('rrt_restarts', 2) rrt_iterations = options.get('rrt_iterations', 20) smooth_iterations = options.get('smooth_iterations', 20) # TODO: auto compute joint weight # print('plan motion options: ', options) # * convert link/joint names to pybullet indices joint_names = robot.get_configurable_joint_names(group=group) ik_joints = joints_from_names(robot_uid, joint_names) joint_types = robot.get_joint_types_by_names(joint_names) pb_custom_limits = get_custom_limits(robot_uid, ik_joints, custom_limits={joint_from_name(robot_uid, jn) : lims for jn, lims in custom_limits.items()}) # print('pb custom limits: ', list(pb_custom_limits)) with WorldSaver(): if start_configuration is not None: # * set to start conf self.client.set_robot_configuration(robot, start_configuration) sample_fn = get_sample_fn(robot_uid, ik_joints, custom_limits=pb_custom_limits) distance_fn = get_distance_fn(robot_uid, ik_joints, weights=weights) extend_fn = get_extend_fn(robot_uid, ik_joints, resolutions=resolutions) options['robot'] = robot collision_fn = PyChoreoConfigurationCollisionChecker(self.client)._get_collision_fn(robot, joint_names, options=options) start_conf = get_joint_positions(robot_uid, ik_joints) end_conf = self._joint_values_from_joint_constraints(joint_names, goal_constraints) assert len(ik_joints) == len(end_conf) if not check_initial_end(start_conf, end_conf, collision_fn, diagnosis=diagnosis): return None path = birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn, collision_fn, restarts=rrt_restarts, iterations=rrt_iterations, smooth=smooth_iterations) #return plan_lazy_prm(start_conf, end_conf, sample_fn, extend_fn, collision_fn) if path is None: # TODO use LOG if verbose: cprint('No free motion found!', 'red') return None else: jt_traj_pts = [] for i, conf in enumerate(path): # c_conf = Configuration(values=conf, types=joint_types, joint_names=joint_names) jt_traj_pt = JointTrajectoryPoint(values=conf, types=joint_types, time_from_start=Duration(i*1,0)) # TODO why don't we have a `joint_names` input for JointTrajectoryPoint? # https://github.com/compas-dev/compas_fab/blob/master/src/compas_fab/robots/trajectory.py#L64 jt_traj_pt.joint_names = joint_names jt_traj_pts.append(jt_traj_pt) trajectory = JointTrajectory(trajectory_points=jt_traj_pts, joint_names=joint_names, start_configuration=jt_traj_pts[0], fraction=1.0) return trajectory
def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): """Calculate the robot's inverse kinematic for a given frame. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which inverse kinematics is being calculated. frame_WCF: :class:`compas.geometry.Frame` The frame to calculate the inverse for. start_configuration: :class:`compas_fab.robots.Configuration`, optional If passed, the inverse will be calculated such that the calculated joint positions differ the least from the start_configuration. Defaults to the init configuration. group: str, optional The planning group used for calculation. Defaults to the robot's main planning group. options: dict, optional Dictionary containing the following key-value pairs: - ``"base_link"``: (:obj:`str`) Name of the base link. - ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions. Defaults to `True`. - ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional) A set of constraints that the request must obey. Defaults to `None`. - ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts. Defaults to `8`. - ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional) Defaults to `None`. - ``"return_all"``: (bool, optional) return a list of all computed ik solutions Defaults to False Raises ------ compas_fab.backends.exceptions.BackendError If no configuration can be found. Returns ------- :class:`compas_fab.robots.Configuration` The planning group's configuration. """ max_iterations = is_valid_option(options, 'attempts', 8) avoid_collisions = is_valid_option(options, 'avoid_collisions', True) return_all = is_valid_option(options, 'return_all', False) custom_limits = options.get('custom_limits') or {} # return_closest_to_start = is_valid_option(options, 'return_closest_to_start', False) # cull = is_valid_option(options, 'cull', True) robot_uid = robot.attributes['pybullet_uid'] ik_joint_names = robot.get_configurable_joint_names(group=group) ik_joints = joints_from_names(robot_uid, ik_joint_names) tool_link_name = robot.get_end_effector_link_name(group=group) tool_link = link_from_name(robot_uid, tool_link_name) pb_custom_limits = get_custom_limits( robot_uid, ik_joints, custom_limits={ joint_from_name(robot_uid, jn): lims for jn, lims in custom_limits.items() }) target_pose = pose_from_frame(frame_WCF) with WorldSaver(): if start_configuration is not None: start_conf_vals = start_configuration.values set_joint_positions(robot_uid, ik_joints, start_conf_vals) # else: # sample_fn = get_sample_fn(robot_uid, ik_joints) # start_conf_vals = sample_fn() # if group not in self.client.planner.ik_fn_from_group: # use default ik fn conf_vals = self._compute_ik(robot_uid, ik_joints, tool_link, target_pose, max_iterations, custom_limits=pb_custom_limits) joint_types = robot.get_joint_types_by_names(ik_joint_names) configurations = [Configuration(values=conf_val, types=joint_types, joint_names=ik_joint_names) \ for conf_val in conf_vals if conf_val is not None] # else: # # qs = client.inverse_kinematics(frame_WCF, group=move_group) # configurations = self.client.planner.ik_fn_from_group[group](frame_WCF, group=group, options=options) if avoid_collisions: configurations = [ conf for conf in configurations if not self.client.check_collisions( robot, conf, options=options) ] if return_all: return configurations else: return configurations[0] if len(configurations) > 0 else None
def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None): """Calculates a cartesian motion path (linear in tool space). Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which the motion path is being calculated. frames_WCF: list of :class:`compas.geometry.Frame` The frames through which the path is defined. start_configuration: :class:`Configuration`, optional The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. group: str, optional The planning group used for calculation. options: dict, optional Dictionary containing kwargs for arguments specific to the client being queried. - ``"diagnosis"``: (:obj:`bool`, optional) . Defaults to ``False``. - ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions. Defaults to ``True``. - ``"planner_id"``: (:obj:`str`) The name of the algorithm used for path planning. Defaults to ``'IterativeIK'``. Available planners: ``'IterativeIK', 'LadderGraph'`` - ``"max_step"``: (:obj:`float`, optional) The approximate distance between the calculated points. (Defined in the robot's units.) Defaults to ``0.01``. - ``"jump_threshold"``: (:obj:`float`, optional) The maximum allowed distance of joint positions between consecutive points. If the distance is found to be above this threshold, the path computation fails. It must be specified in relation to max_step. If this threshold is ``0``, 'jumps' might occur, resulting in an invalid cartesian path. Defaults to :math:`\\pi / 6` for revolute or continuous joints, 0.1 for prismatic joints. # TODO: JointConstraint Returns ------- :class:`compas_fab.robots.JointTrajectory` The calculated trajectory. """ robot_uid = robot.attributes['pybullet_uid'] # * convert link/joint names to pybullet indices base_link_name = robot.get_base_link_name(group=group) tool_link_name = robot.get_end_effector_link_name(group=group) tool_link = link_from_name(robot_uid, tool_link_name) base_link = link_from_name(robot_uid, base_link_name) joint_names = robot.get_configurable_joint_names(group=group) joint_types = robot.get_joint_types_by_names(joint_names) ik_joints = joints_from_names(robot_uid, joint_names) # * parse options verbose = options.get('verbose', True) diagnosis = options.get('diagnosis', False) avoid_collisions = options.get('avoid_collisions', True) pos_step_size = options.get('max_step', 0.01) planner_id = is_valid_option(options, 'planner_id', 'IterativeIK') jump_threshold = is_valid_option(options, 'jump_threshold', {jt_name : math.pi/6 \ if jt_type in [Joint.REVOLUTE, Joint.CONTINUOUS] else 0.1 \ for jt_name, jt_type in zip(joint_names, joint_types)}) jump_threshold_from_joint = { joint_from_name(robot_uid, jt_name): j_diff for jt_name, j_diff in jump_threshold.items() } # * iterative IK options pos_tolerance = is_valid_option(options, 'pos_tolerance', 1e-3) ori_tolerance = is_valid_option(options, 'ori_tolerance', 1e-3 * np.pi) # * ladder graph options frame_variant_gen = is_valid_option(options, 'frame_variant_generator', None) ik_function = is_valid_option(options, 'ik_function', None) # * convert to poses and do workspace linear interpolation given_poses = [pose_from_frame(frame_WCF) for frame_WCF in frames_WCF] ee_poses = [] for p1, p2 in zip(given_poses[:-1], given_poses[1:]): c_interp_poses = list( interpolate_poses(p1, p2, pos_step_size=pos_step_size)) ee_poses.extend(c_interp_poses) # * build collision fn attachments = values_as_list(self.client.pychoreo_attachments) collision_fn = PyChoreoConfigurationCollisionChecker( self.client)._get_collision_fn(robot, joint_names, options=options) failure_reason = '' with WorldSaver(): # set to start conf if start_configuration is not None: self.client.set_robot_configuration(robot, start_configuration) if planner_id == 'IterativeIK': selected_links = [ link_from_name(robot_uid, l) for l in robot.get_link_names(group=group) ] # with HideOutput(): # with redirect_stdout(): path = plan_cartesian_motion_from_links( robot_uid, selected_links, tool_link, ee_poses, get_sub_conf=False, pos_tolerance=pos_tolerance, ori_tolerance=ori_tolerance) if path is None: failure_reason = 'IK plan is not found.' # collision checking is not included in the default Cartesian planning if path is not None and avoid_collisions: for i, conf_val in enumerate(path): pruned_conf_val = self._prune_configuration( robot_uid, conf_val, joint_names) for attachment in attachments: attachment.assign() if collision_fn(pruned_conf_val, diagnosis=diagnosis): failure_reason = 'IK plan is found but collision violated.' path = None break path[i] = pruned_conf_val # TODO check joint threshold elif planner_id == 'LadderGraph': # get ik fn from client # collision checking is turned off because collision checking is handled inside LadderGraph planner ik_options = {'avoid_collisions': False, 'return_all': True} sample_ik_fn = ik_function or self._get_sample_ik_fn( robot, ik_options) # convert ee_variant_fn if frame_variant_gen is not None: def sample_ee_fn(pose): for v_frame in frame_variant_gen.generate_frame_variant( frame_from_pose(pose)): yield pose_from_frame(v_frame) else: sample_ee_fn = None path, cost = plan_cartesian_motion_lg(robot_uid, ik_joints, ee_poses, sample_ik_fn, collision_fn, \ jump_threshold=jump_threshold_from_joint, sample_ee_fn=sample_ee_fn) if verbose: print('Ladder graph cost: {}'.format(cost)) else: raise ValueError('Cartesian planner {} not implemented!', planner_id) if path is None: if verbose: cprint( 'No Cartesian motion found, due to {}!'.format( failure_reason), 'red') return None else: # TODO start_conf might have different number of joints with the given group? start_traj_pt = None if start_configuration is not None: start_traj_pt = JointTrajectoryPoint( values=start_configuration.values, types=start_configuration.types) start_traj_pt.joint_names = start_configuration.joint_names jt_traj_pts = [] for i, conf in enumerate(path): jt_traj_pt = JointTrajectoryPoint(values=conf, types=joint_types) jt_traj_pt.joint_names = joint_names if start_traj_pt is not None: # ! TrajectoryPoint doesn't copy over joint_names... jtp = start_traj_pt.copy() jtp.joint_names = start_traj_pt.joint_names jtp.merge(jt_traj_pt) jt_traj_pt = jtp jt_traj_pt.time_from_start = Duration(i * 1, 0) jt_traj_pts.append(jt_traj_pt) if start_configuration is not None and not compare_configurations( start_configuration, jt_traj_pts[0], jump_threshold, verbose=verbose): # if verbose: # print() # cprint('Joint jump from start conf, max diff {}'.format(start_configuration.max_difference(jt_traj_pts[0])), 'red') # cprint('start conf {}'.format(['{:.4f}'.format(v) for v in start_configuration.values]), 'red') # cprint('traj pt 0 {}'.format(['{:.4f}'.format(v) for v in jt_traj_pts[0].values]), 'red') pass # return None # TODO check intermediate joint jump trajectory = JointTrajectory( trajectory_points=jt_traj_pts, joint_names=jt_traj_pts[0].joint_names, start_configuration=jt_traj_pts[0], fraction=1.0) return trajectory
def sequenced_picknplace_plan(assembly_pkg_json_path, solve_method='sparse_ladder_graph', viewer=False, scale=1e-3, sample_time=5, sparse_time_out=2, jt_res=0.1, pos_step_size=0.01, ori_step_size=np.pi/16, viz_inspect=False, warning_pause=False, save_dir=None, **kwargs): """call pychoreo's planner to solve for picknplace Cartesian & transition trajectories. Robot setup is specified in coop_assembly.choreo_interface.robot_setup Parameters ---------- assembly_pkg_json_path : [type] [description] solve_method : str, optional [description], by default 'sparse_ladder_graph' viewer : bool, optional [description], by default False scale : [type], optional [description], by default 1e-3 sample_time : int, optional [description], by default 5 sparse_time_out : int, optional [description], by default 2 jt_res : float, optional joint resolution for transition planning, by default 0.1 pos_step_size : float, optional interpolation step size for the end effector Cartesia movements, by default 0.01 (meter) ori_step_size : [type], optional [description], by default np.pi/16 viz_inspect : bool, optional visualize planning process in a pybullet window (slow!), by default False warning_pause : bool, optional wait for user input if any of the planning process cannot find a solution, by default False save_dir : string, optional absolute directory path to save the planning result json file, by default None Returns ------- [type] [description] Raises ------ ValueError [description] """ # TODO: assert solve method in avaiable list # * load robot setup data (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) = get_picknplace_robot_data() picknplace_end_effector_urdf = get_picknplace_end_effector_urdf() picknplace_tcp_def = get_picknplace_tcp_def() # * create robot and pb environment connect(use_gui=viewer) # * adjust camera pose (optional) if has_gui(): 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) with HideOutput(): # * pybullet can handle ROS-package path URDF automatically now (ver 2.5.7)! robot = load_pybullet(robot_urdf, fixed_base=True) workspace = load_pybullet(workspace_urdf, fixed_base=True) # ee_body = create_obj(picknplace_end_effector_urdf) ee_body = load_pybullet(picknplace_end_effector_urdf) # * set robot idle configuration ik_joints = joints_from_names(robot, ik_joint_names) robot_start_conf = get_robot_init_conf() set_joint_positions(robot, ik_joints, robot_start_conf) # * create tool and tool TCP from flange (tool0) transformation root_link = link_from_name(robot, tool_root_link_name) # create end effector body ee_attach = Attachment(robot, root_link, unit_pose(), ee_body) # set up TCP transformation, just a renaming here root_from_tcp = picknplace_tcp_def if has_gui() : # draw_tcp pose ee_attach.assign() ee_link_pose = get_pose(ee_attach.child) draw_pose(multiply(ee_link_pose, root_from_tcp)) # * specify ik fn wrapper ik_fn = IK_MODULE.get_ik def get_sample_ik_fn(robot, ik_fn, ik_joint_names, base_link_name, tool_from_root=None): def sample_ik_fn(world_from_tcp): if tool_from_root: world_from_tcp = multiply(world_from_tcp, tool_from_root) return sample_tool_ik(ik_fn, robot, ik_joint_names, base_link_name, world_from_tcp, get_all=True) #,sampled=[0]) return sample_ik_fn # ik generation function stays the same for all cartesian processes sample_ik_fn = get_sample_ik_fn(robot, ik_fn, ik_joint_names, base_link_name, invert(root_from_tcp)) # * load shape & collision data with open(assembly_pkg_json_path, 'r') as f: json_data = json.loads(f.read()) assembly = Assembly.from_package(json_data) elements = assembly.elements for element in elements.values(): for unit_geo in element.unit_geometries: unit_geo.rescale(scale) # TODO: scale derived from the assembly package unit # static_obstacles = [] # for ug in assembly.static_obstacle_geometries.values(): # static_obstacles.extend(ug.mesh) # * load precomputed sequence / use assigned sequence # TODO: load this as function argument # element_seq = elements.keys() element_seq = [0, 1, 2, 3, 4, 5] # element_seq = [3, 4, 5] print('sequence: ', element_seq) # visualize goal pose if has_gui(): # viz_len = 0.003 with WorldSaver(): for e_id in element_seq: element = elements[e_id] with LockRenderer(): print('e_id #{} : {}'.format(e_id, element)) for unit_geo in element.unit_geometries: for pb_geo in unit_geo.pybullet_bodies: set_pose(pb_geo, random.choice(unit_geo.get_goal_frames(get_pb_pose=True))) # print('---------') # wait_for_user() # wait_for_user() # * construct ignored body-body links for collision checking # in this case, including self-collision between links of the robot disabled_self_collisions = get_disabled_collisions(robot, disabled_self_collision_link_names) # and links between the robot and the workspace (e.g. robot_base_link to base_plate) extra_disabled_collisions = get_body_body_disabled_collisions(robot, workspace, workspace_robot_disabled_link_names) # TODO: extra disabled collisions as function argument extra_disabled_collisions.update({ ((robot, link_from_name(robot, 'robot_link_5')), (ee_body, link_from_name(ee_body, 'eef_base_link'))), ((robot, link_from_name(robot, 'robot_link_6')), (ee_body, link_from_name(ee_body, 'eef_base_link'))) }) # * create cartesian processes without a sequence being given, with random pose generators cart_process_seq = build_picknplace_cartesian_process_seq( element_seq, elements, robot, ik_joint_names, root_link, sample_ik_fn, ee_attachs=[ee_attach], self_collisions=True, disabled_collisions=disabled_self_collisions, obstacles=[workspace],extra_disabled_collisions=extra_disabled_collisions, tool_from_root=invert(root_from_tcp), viz_step=False, pick_from_same_rack=True, pos_step_size=pos_step_size, ori_step_size=ori_step_size) # specifically for UR5, because of its wide joint range, we need to apply joint value snapping for cp in cart_process_seq: cp.target_conf = robot_start_conf with LockRenderer(not viz_inspect): if solve_method == 'ladder_graph': print('\n'+'#' * 10) print('Solving with the vanilla ladder graph search algorithm.') cart_process_seq = solve_ladder_graph_from_cartesian_process_list(cart_process_seq, verbose=True, warning_pause=warning_pause, viz_inspect=viz_inspect, check_collision=False, start_conf=robot_start_conf) elif solve_method == 'sparse_ladder_graph': print('\n'+'#' * 10) print('Solving with the sparse ladder graph search algorithm.') sparse_graph = SparseLadderGraph(cart_process_seq) sparse_graph.find_sparse_path(verbose=True, vert_timeout=sample_time, sparse_sample_timeout=sparse_time_out) cart_process_seq = sparse_graph.extract_solution(verbose=True, start_conf=robot_start_conf) else: raise ValueError('Invalid solve method!') assert all(isinstance(cp, CartesianProcess) for cp in cart_process_seq) pnp_trajs = [[] for _ in range(len(cart_process_seq))] for cp_id, cp in enumerate(cart_process_seq): element_attachs = [] for sp_id, sp in enumerate(cp.sub_process_list): assert sp.trajectory, '{}-{} does not have a Cartesian plan found!'.format(cp, sp) # ! reverse engineer the grasp pose if sp.trajectory.tag == 'pick_retreat': unit_geo = elements[sp.trajectory.element_id].unit_geometries[0] e_bodies = unit_geo.pybullet_bodies for e_body in e_bodies: set_pose(e_body, unit_geo.get_initial_frames(get_pb_pose=True)[0]) set_joint_positions(sp.trajectory.robot, sp.trajectory.joints, sp.trajectory.traj_path[0]) element_attachs.append(create_attachment(sp.trajectory.robot, root_link, e_body)) if sp.trajectory.tag == 'pick_retreat' or sp.trajectory.tag == 'place_approach': sp.trajectory.attachments= element_attachs pnp_trajs[cp_id].append(sp.trajectory) full_trajs = pnp_trajs # * transition motion planning between extrusions return2idle = True transition_traj = solve_transition_between_picknplace_processes(pnp_trajs, elements, robot_start_conf, disabled_collisions=disabled_self_collisions, extra_disabled_collisions=extra_disabled_collisions, obstacles=[workspace], return2idle=return2idle, resolutions=[jt_res]*len(ik_joints), **kwargs) # * weave the Cartesian and transition processses together for cp_id, print_trajs in enumerate(full_trajs): print_trajs.insert(0, transition_traj[cp_id][0]) print_trajs.insert(3, transition_traj[cp_id][1]) if return2idle: full_trajs[-1].append(transition_traj[-1][-1]) if save_dir is None: here = os.path.dirname(__file__) save_dir = os.path.join(here, 'results') export_trajectory(save_dir, full_trajs, ee_link_name, indent=1, shape_file_path=assembly_pkg_json_path, include_robot_data=True, include_link_path=True) # * disconnect and close pybullet engine used for planning, visualizing trajectories will start a new one reset_simulation() disconnect() if viewer: cart_time_step = None tr_time_step = None display_picknplace_trajectories(robot_urdf, ik_joint_names, assembly_pkg_json_path, full_trajs, tool_root_link_name, ee_urdf=picknplace_end_effector_urdf, workspace_urdf=workspace_urdf, animate=True, cart_time_step=cart_time_step, tr_time_step=tr_time_step)
def rfl_demo(viewer=True): arm = 'right' connect(use_gui=viewer) robot = load_pybullet(RFL_ROBOT_URDF, fixed_base=True) set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0)) cprint('hello RFL! <ctrl+left mouse> to pan', 'green') wait_for_user() # create a box to be picked up # see: https://pybullet-planning.readthedocs.io/en/latest/reference/generated/pybullet_planning.interfaces.env_manager.create_box.html#pybullet_planning.interfaces.env_manager.create_box block = create_box(0.059, 20., 0.089) block_x = 10 block_y = 5. block_z = 1.5 set_pose( block, Pose(Point(x=block_x, y=block_y, z=block_z), Euler(yaw=np.pi / 2))) arm_tag = arm[0] arm_joint_names = ['gantry_x_joint', '{}_gantry_y_joint'.format(arm_tag), '{}_gantry_z_joint'.format(arm_tag)] + \ ['{}_robot_joint_{}'.format(arm_tag, i+1) for i in range(6)] arm_joints = joints_from_names(robot, arm_joint_names) # * if a subset of joints is used, use: # arm_joints = joints_from_names(robot, arm_joint_names[1:]) # this will disable the gantry-x joint cprint('Used joints: {}'.format(get_joint_names(robot, arm_joints)), 'yellow') # * get a joint configuration sample function: # it separately sample each joint value within the feasible range sample_fn = get_sample_fn(robot, arm_joints) cprint( 'Randomly sample robot configuration and set them! (no collision check is performed)', 'blue') wait_for_user() for i in range(5): # randomly sample a joint conf sampled_conf = sample_fn() set_joint_positions(robot, arm_joints, sampled_conf) cprint('#{} | Conf sampeld: {}'.format(i, sampled_conf), 'green') wait_for_user() # * now let's plan a trajectory # we use y-z-arm 6 joint all together here cprint( 'Randomly sample robot start/end configuration and comptue a motion plan! (no self-collision check is performed)', 'blue') print( 'Disabled collision links needs to be given (can be parsed from a SRDF via compas_fab)' ) for _ in range(5): print('=' * 10) q1 = list(sample_fn()) # intentionly make the robot needs to cross the collision object # let it start from the right side q1[0] = 0. q1[1] = 0 set_joint_positions(robot, arm_joints, q1) cprint('Sampled start conf: {}'.format(q1), 'cyan') wait_for_user() # let it ends at the left side q2 = list(sample_fn()) q2[0] = 0.5 q2[1] = 7.0 cprint('Sampled end conf: {}'.format(q2), 'cyan') path = plan_joint_motion(robot, arm_joints, q2, obstacles=[block], self_collisions=False, custom_limits={arm_joints[0]: [0.0, 1.2]}) if path is None: cprint('no plan found', 'red') continue else: wait_for_user( 'a motion plan is found! Press enter to start simulating!') # adjusting this number will adjust the simulation speed time_step = 0.03 for conf in path: set_joint_positions(robot, arm_joints, conf) wait_for_duration(time_step)
def _set_body_configuration(self, body_id, configuration): joints = joints_from_names(body_id, configuration.joint_names) set_joint_positions(body_id, joints, configuration.values)
def test_ik(fixed_waam_setup, viewer, ik_engine): urdf_filename, semantics, robotA_tool = fixed_waam_setup move_group = 'robotA' with PyChoreoClient(viewer=viewer) as client: robot = client.load_robot(urdf_filename) robot.semantics = semantics robot_uid = client.get_robot_pybullet_uid(robot) base_link_name = robot.get_base_link_name(group=move_group) ik_joint_names = robot.get_configurable_joint_names(group=move_group) tool_link_name = robot.get_end_effector_link_name(group=move_group) base_frame = robot.get_base_frame(group=move_group) if ik_engine == 'default-single': ik_solver = None elif ik_engine == 'lobster-analytical': ik_solver = InverseKinematicsSolver(robot, move_group, ik_abb_irb4600_40_255, base_frame, robotA_tool.frame) elif ik_engine == 'ikfast-analytical': ikfast_fn = get_ik_fn_from_ikfast(ikfast_abb_irb4600_40_255.get_ik) ik_solver = InverseKinematicsSolver(robot, move_group, ikfast_fn, base_frame, robotA_tool.frame) else: raise ValueError('invalid ik engine name.') ik_joints = joints_from_names(robot_uid, ik_joint_names) tool_link = link_from_name(robot_uid, tool_link_name) ee_poses = compute_circle_path() if ik_solver is not None: # replace default ik function with a customized one client.planner.inverse_kinematics = ik_solver.inverse_kinematics_function( ) ik_time = 0 failure_cnt = 0 # ik function sanity check for p in ee_poses: frame_WCF = frame_from_pose(p) st_time = time.time() qs = client.inverse_kinematics(robot, frame_WCF, group=move_group, options={}) ik_time += elapsed_time(st_time) if qs is None: cprint('no ik solution found!', 'red') # assert False, 'no ik solution found!' failure_cnt += 1 elif isinstance(qs, list): if not (len(qs) > 0 and any([qv is not None for qv in qs])): cprint('no ik solution found', 'red') failure_cnt += 1 if len(qs) > 0: # cprint('{} solutions found!'.format(len(qs)), 'green') for q in randomize(qs): if q is not None: assert isinstance(q, Configuration) client.set_robot_configuration(robot, q) # set_joint_positions(robot_uid, ik_joints, q.values) tcp_pose = get_link_pose(robot_uid, tool_link) assert_almost_equal(tcp_pose[0], p[0], decimal=3) assert_almost_equal(quat_angle_between( tcp_pose[1], p[1]), 0, decimal=3) elif isinstance(qs, Configuration): # cprint('Single solutions found!', 'green') q = qs # set_joint_positions(robot_uid, ik_joints, q.values) client.set_robot_configuration(robot, q) tcp_pose = get_link_pose(robot_uid, tool_link) assert_almost_equal(tcp_pose[0], p[0], decimal=3) assert_almost_equal(quat_angle_between(tcp_pose[1], p[1]), 0, decimal=3) else: raise ValueError('invalid ik return.') wait_if_gui('FK - IK agrees.') cprint( '{} | Success {}/{} | Average ik time: {} | avg over {} calls.'. format(ik_engine, len(ee_poses) - failure_cnt, len(ee_poses), ik_time / len(ee_poses), len(ee_poses)), 'cyan')
def test_circle_cartesian(fixed_waam_setup, viewer, planner_ik_conf): urdf_filename, semantics, robotA_tool = fixed_waam_setup planner_id, ik_engine = planner_ik_conf move_group = 'robotA' print('\n') print('=' * 10) cprint( 'Cartesian planner {} with IK engine {}'.format(planner_id, ik_engine), 'yellow') with PyChoreoClient(viewer=viewer) as client: robot = client.load_robot(urdf_filename) robot.semantics = semantics robot_uid = client.get_robot_pybullet_uid(robot) base_link_name = robot.get_base_link_name(group=move_group) ik_joint_names = robot.get_configurable_joint_names(group=move_group) tool_link_name = robot.get_end_effector_link_name(group=move_group) base_frame = robot.get_base_frame(group=move_group) if ik_engine == 'default-single': ik_solver = None elif ik_engine == 'lobster-analytical': ik_solver = InverseKinematicsSolver(robot, move_group, ik_abb_irb4600_40_255, base_frame, robotA_tool.frame) elif ik_engine == 'ikfast-analytical': ikfast_fn = get_ik_fn_from_ikfast(ikfast_abb_irb4600_40_255.get_ik) ik_solver = InverseKinematicsSolver(robot, move_group, ikfast_fn, base_frame, robotA_tool.frame) else: raise ValueError('invalid ik engine name.') init_conf = Configuration.from_revolute_values(np.zeros(6), ik_joint_names) # replace default ik function with a customized one if ik_solver is not None: client.planner.inverse_kinematics = ik_solver.inverse_kinematics_function( ) tool_link = link_from_name(robot_uid, tool_link_name) robot_base_link = link_from_name(robot_uid, base_link_name) ik_joints = joints_from_names(robot_uid, ik_joint_names) # * draw EE pose tcp_pose = get_link_pose(robot_uid, tool_link) draw_pose(tcp_pose) # * generate multiple circles circle_center = np.array([2, 0, 0.2]) circle_r = 0.2 # full_angle = np.pi # full_angle = 2*2*np.pi angle_range = (-0.5 * np.pi, 0.5 * np.pi) # total num of path pts, one path point per 5 degree ee_poses = compute_circle_path(circle_center, circle_r, angle_range) ee_frames_WCF = [frame_from_pose(ee_pose) for ee_pose in ee_poses] options = {'planner_id': planner_id} if planner_id == 'LadderGraph': client.set_robot_configuration(robot, init_conf) st_time = time.time() # options.update({'ik_function' : lambda pose: compute_inverse_kinematics(ikfast_abb_irb4600_40_255.get_ik, pose, sampled=[])}) trajectory = client.plan_cartesian_motion(robot, ee_frames_WCF, group=move_group, options=options) cprint( 'W/o frame variant solving time: {}'.format( elapsed_time(st_time)), 'blue') cprint( 'Cost: {}'.format( compute_trajectory_cost(trajectory, init_conf_val=init_conf.values)), 'blue') print('-' * 5) f_variant_options = {'delta_yaw': np.pi / 3, 'yaw_sample_size': 5} options.update({ 'frame_variant_generator': PyChoreoFiniteEulerAngleVariantGenerator( options=f_variant_options) }) print('With frame variant config: {}'.format(f_variant_options)) client.set_robot_configuration(robot, init_conf) st_time = time.time() trajectory = client.plan_cartesian_motion(robot, ee_frames_WCF, group=move_group, options=options) cprint( '{} solving time: {}'.format( 'With frame variant ' if planner_id == 'LadderGraph' else 'Direct', elapsed_time(st_time)), 'cyan') cprint( 'Cost: {}'.format( compute_trajectory_cost(trajectory, init_conf_val=init_conf.values)), 'cyan') if trajectory is None: cprint( 'Client Cartesian planner {} CANNOT find a plan!'.format( planner_id), 'red') else: cprint( 'Client Cartesian planning {} find a plan!'.format(planner_id), '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()
def test_collision_checker(rfl_setup, itj_beam_cm, viewer, diagnosis): # modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py urdf_filename, semantics = rfl_setup move_group = 'robot11_eaXYZ' # 'robot12_eaYZ' with PyChoreoClient(viewer=viewer) as client: cprint(urdf_filename, 'green') with LockRenderer(): robot = client.load_robot(urdf_filename) robot.semantics = semantics robot_uid = client.get_robot_pybullet_uid(robot) 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) ee_touched_link_names = ['robot12_tool0', 'robot12_link_6'] # ee_acms = [AttachedCollisionMesh(ee_cm, flange_link_name, ee_touched_link_names) for ee_cm in itj_TC_PG500_cms] # beam_acm = AttachedCollisionMesh(itj_beam_cm, flange_link_name, ee_touched_link_names) draw_pose(unit_pose(), length=1.) cam = rfl_camera() set_camera_pose(cam['target'], cam['location']) ik_joints = joints_from_names(robot_uid, ik_joint_names) flange_link = link_from_name(robot_uid, flange_link_name) # robot_base_link = link_from_name(robot_uid, base_link_name) r11_start_conf_vals = np.array([22700.0, 0.0, -4900.0, \ 0.0, -80.0, 65.0, 65.0, 20.0, -20.0]) r12_start_conf_vals = np.array([-4056.0883789999998, -4000.8486330000001, \ 0.0, -22.834741999999999, -30.711554, 0.0, 57.335655000000003, 0.0]) full_start_conf = to_rlf_robot_full_conf(r11_start_conf_vals, r12_start_conf_vals) # full_joints = joints_from_names(robot_uid, full_start_conf.joint_names) client.set_robot_configuration(robot, full_start_conf) # # * add attachment # for ee_acm in ee_acms: # client.add_attached_collision_mesh(ee_acm, options={'robot' : robot, 'mass': 1}) # client.add_attached_collision_mesh(beam_acm, options={'robot' : robot, 'mass': 1}) # safe start conf start_confval = np.hstack([ r11_start_conf_vals[:3] * 1e-3, np.radians(r11_start_conf_vals[3:]) ]) conf = Configuration(values=start_confval.tolist(), types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf, options={'diagnosis': diagnosis}) # TODO add more tests from pybullet_planning import get_link_subtree, prune_fixed_joints, clone_body, get_movable_joints, get_all_links, \ set_color, child_link_from_joint first_joint = ik_joints[0] target_link = flange_link # selected_links = get_link_subtree(robot_uid, first_joint) # TODO: child_link_from_joint? selected_links = [child_link_from_joint(joint) for joint in ik_joints] selected_movable_joints = prune_fixed_joints(robot_uid, selected_links) assert (target_link in selected_links) selected_target_link = selected_links.index(target_link) sub_robot = clone_body(robot_uid, links=selected_links, visual=True, collision=True) # TODO: joint limits sub_movable_joints = get_movable_joints(sub_robot) # conf = Configuration(values=[0.]*6, types=ik_joint_types, joint_names=ik_joint_names) # assert not client.configuration_in_collision(conf, group=move_group, options={'diagnosis':True}) wait_if_gui()