def generate_frame_variant(self, frame, options=None): """.... Parameters ---------- frame : :class:`compas.geometry.Frame` TODO Yield ----- frame frame variantion """ # overwrite if options is provided in the function call options = options or self._options # * half range delta_roll = is_valid_option(options, 'delta_roll', 0.) delta_pitch = is_valid_option(options, 'delta_pitch', 0.) delta_yaw = is_valid_option(options, 'delta_yaw', 0.) # * discretization roll_sample_size = is_valid_option(options, 'roll_sample_size', 1) pitch_sample_size = is_valid_option(options, 'pitch_sample_size', 1) yaw_sample_size = is_valid_option(options, 'yaw_sample_size', 1) roll_gen = np.linspace(-delta_roll, delta_roll, num=roll_sample_size) pitch_gen = np.linspace(-delta_pitch, delta_pitch, num=pitch_sample_size) yaw_gen = np.linspace(-delta_yaw, delta_yaw, num=yaw_sample_size) pose = pose_from_frame(frame) # a finite generator for roll, pitch, yaw in product(roll_gen, pitch_gen, yaw_gen): yield frame_from_pose(multiply(pose, Pose(euler=Euler(roll=roll, pitch=pitch, yaw=yaw))))
def test_frame_variant_generator(viewer): pose = unit_pose() frame = frame_from_pose(pose) options = {'delta_yaw': np.pi / 6, 'yaw_sample_size': 30} frame_gen = PyChoreoFiniteEulerAngleVariantGenerator( options).generate_frame_variant with PyChoreoClient(viewer=viewer) as client: draw_pose(pose) cnt = 0 for frame in frame_gen(frame): draw_pose(pose_from_frame(frame)) cnt += 1 assert cnt == options['yaw_sample_size'] wait_if_gui() remove_all_debug() # overwrite class options cnt = 0 new_options = {'delta_yaw': np.pi / 3, 'yaw_sample_size': 60} for frame in frame_gen(frame, new_options): draw_pose(pose_from_frame(frame)) cnt += 1 assert cnt == new_options['yaw_sample_size'] wait_if_gui()
def sample_ik_fn(pose): # pb pose -> list(conf values) # TODO run random seed here and return a list of confs configurations = self.client.inverse_kinematics( robot, frame_from_pose(pose), options=ik_options) return [ configuration.values for configuration in configurations if configuration 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)
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 fn(pose): configurations = compas_fab_ik_fn(frame_from_pose(pose)) return [ np.array(configuration.values) for configuration in configurations if configuration is not None ]
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()