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()