def data(self, data): self.values = data.get('values') or [] self.types = data.get('types') or [] self.velocities = data.get('velocities') or [] self.accelerations = data.get('accelerations') or [] self.effort = data.get('effort') or [] self.time_from_start = Duration.from_data(data.get('time_from_start') or {})
def data(self, data): self._joint_values = FixedLengthList( data.get('joint_values') or data.get('values') or []) self._joint_types = FixedLengthList( data.get('joint_types') or data.get('types') or []) self._joint_names = FixedLengthList(data.get('joint_names') or []) self._velocities = FixedLengthList(data.get('velocities') or []) self._accelerations = FixedLengthList(data.get('accelerations') or []) self._effort = FixedLengthList(data.get('effort') or []) self.time_from_start = Duration.from_data( data.get('time_from_start') or {})
def __init__(self, values=None, types=None, velocities=None, accelerations=None, effort=None, time_from_start=None): super(JointTrajectoryPoint, self).__init__(values, types) self.velocities = velocities or len(self.values) * [0.] self.accelerations = accelerations or len(self.values) * [0.] self.effort = effort or len(self.values) * [0.] self.time_from_start = time_from_start or Duration(0, 0)
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 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 traj_reparam(compas_fab_jt_traj, max_jt_vel, max_jt_acc, traj_time_cnt=0, ts_sample_num=100, grid_num=200, inspect_sol=False): dof = len(compas_fab_jt_traj.points[0].values) # Create path way_jt_pts = [jt_pt.values for jt_pt in compas_fab_jt_traj.points] path = ta.SplineInterpolator( np.linspace(0, 1, len(compas_fab_jt_traj.points)), way_jt_pts) # Create velocity bounds, then velocity constraint object vlim_ = np.ones(6) * max_jt_vel vlim = np.vstack((-vlim_, vlim_)).T # Create acceleration bounds, then acceleration constraint object alim_ = np.ones(6) * max_jt_acc alim = np.vstack((-alim_, alim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) # Setup a parametrization instance, then retime gridpoints = np.linspace(0, path.duration, grid_num) instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints, solver_wrapper='seidel') jnt_traj, aux_traj, int_data = instance.compute_trajectory( 0, 0, return_data=True) if inspect_sol: ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qdds_sample = jnt_traj.evaldd(ts_sample) qds_sample = jnt_traj.evald(ts_sample) fig, axs = plt.subplots(1, 2, sharex=True, figsize=[12, 4]) for i in range(6): axs[0].plot(ts_sample, qdds_sample[:, i], label="J{:d}".format(i + 1)) axs[1].plot(ts_sample, qds_sample[:, i], label="J{:d}".format(i + 1)) axs[0].set_xlabel("Time (s)") axs[0].set_ylabel("Joint acceleration (rad/s^2)") axs[0].legend() axs[1].legend() axs[1].set_xlabel("Time (s)") axs[1].set_ylabel("Joint velocity (rad/s)") plt.show() time_list = np.linspace(0, float(jnt_traj.get_duration()), ts_sample_num) jt_list = jnt_traj.eval(time_list) vel_list = jnt_traj.evald(time_list) acc_list = jnt_traj.evaldd(time_list) reparm_traj_pts = [] for i in range(ts_sample_num): new_jt_pt = JointTrajectoryPoint(values=jt_list[i].tolist(), types=[0] * 6) new_jt_pt.velocities = vel_list[i].tolist() new_jt_pt.accelerations = acc_list[i].tolist() new_jt_pt.time_from_start = Duration.from_seconds(traj_time_cnt) traj_time_cnt += time_list[i] reparm_traj_pts.append(new_jt_pt) if i == 0 and np.array_equal(time_list, np.zeros(ts_sample_num)): break reparam_traj = JointTrajectory(trajectory_points=reparm_traj_pts, start_configuration=reparm_traj_pts[0]) return reparam_traj
data.get('start_configuration')) self.fraction = data.get('fraction') @property def time_from_start(self): """Effectively, time from start for the last point in the trajectory. """ if not self.points: return 0. return self.points[-1].time_from_start.seconds if __name__ == '__main__': p1 = JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6) p1.time_from_start = Duration(2, 1293) p2 = JointTrajectoryPoint([0.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6) p2.time_from_start = Duration(6, 0) trj = JointTrajectory([p1, p2]) print(trj) # p.accelerations = [2,3,4,3,5,1] # p.time_from_start = [Duration(1, 23)] # print(p1.to_data()) # print(JointTrajectoryPoint.from_data(p1.to_data())) print(trj.points) print(trj.to_data()) print(JointTrajectory.from_data(trj.to_data()).to_data())