def _get_collision_checking_setup(self, options=None):
avoid_collisions = options.get('avoid_collisions', True)
if avoid_collisions:
wildcards = options.get('collision_object_wildcards') or None
if wildcards is None:
# consider all of them
obstacles = values_as_list(self.collision_objects)
else:
obstacles = []
for wc in wildcards:
names = wildcard_keys(self.collision_objects, wc)
for n in names:
obstacles.extend(self.collision_objects[n])
# ! doesn't make sense to have a wildcard selection for attached objects
attachments = values_as_list(self.pychoreo_attachments)
# TODO additional disabled collisions in options
extra_disabled_collision_names = values_as_list(
self.extra_disabled_collision_links)
option_disabled_link_names = options.get(
'extra_disabled_collisions') or set()
extra_disabled_collisions = set()
for bpair in list(extra_disabled_collision_names) + list(
option_disabled_link_names):
b1, b1link_name = bpair[0]
b2, b2link_name = bpair[1]
b1_links = get_all_links(b1) if b1link_name is None else [
link_from_name(b1, b1link_name)
]
b2_links = get_all_links(b2) if b2link_name is None else [
link_from_name(b2, b2link_name)
]
for b1_link, b2_link in product(b1_links, b2_links):
extra_disabled_collisions.add(
((b1, b1_link), (b2, b2_link)))
else:
# only check joint limits, no collision considered
obstacles = []
attachments = []
return obstacles, attachments, extra_disabled_collisions
def test_client(fixed_waam_setup, viewer):
# https://github.com/gramaziokohler/algorithmic_details/blob/e1d5e24a34738822638a157ca29a98afe05beefd/src/algorithmic_details/accessibility/reachability_map.py#L208-L231
urdf_filename, semantics, _ = 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)
tool_link_name = robot.get_end_effector_link_name(group=move_group)
# * draw EE pose
tool_link = link_from_name(robot_uid, tool_link_name)
tcp_pose = get_link_pose(robot_uid, tool_link)
draw_pose(tcp_pose)
assert robot_uid in get_bodies()
wait_if_gui()
def get_camera_images(self):
eyeid = pyplan.link_from_name(self.id, 'torso_camera_link')
fov, aspect, nearplane, farplane = 80, 1.0, 0.01, 100
projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, nearplane, farplane)
com_p, com_o, _, _, _, _ = p.getLinkState(self.id, eyeid)
rot_matrix = p.getMatrixFromQuaternion(
com_o) #p.getQuaternionFromEuler((1.5707,0.866,0)))
rot_matrix = np.array(rot_matrix).reshape(3, 3)
# Initial vectors
init_camera_vector = (1, 0, 0) # z-axis
init_up_vector = (0, 1, 0) # y-axis
# Rotated vectors
camera_vector = rot_matrix.dot(init_camera_vector)
up_vector = rot_matrix.dot(init_up_vector)
view_matrix = p.computeViewMatrix(com_p + 0.25 * camera_vector,
com_p + 100 * camera_vector,
up_vector)
imgs = p.getCameraImage(640, 640, view_matrix, projection_matrix)
return imgs
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 descartes_demo(viewer=True, scaling_test=False, draw_graph=True):
connect(use_gui=viewer)
with HideOutput():
robot = load_pybullet(KUKA_ROBOT_URDF, fixed_base=True)
# * adjust camera pose (optional)
# has_gui checks if the GUI mode is enabled
if has_gui():
camera_base_pt = (0,0,0)
camera_pt = np.array(camera_base_pt) + np.array([1, -0.5, 1])
set_camera_pose(tuple(camera_pt), camera_base_pt)
cprint('Hello robot world! <ctrl+left mouse> to pan', 'green')
wait_if_gui()
ik_joints = get_movable_joints(robot)
ik_joint_names = get_joint_names(robot, ik_joints)
print('ik joint names: {}'.format(ik_joint_names))
lower_limits, upper_limits = get_custom_limits(robot, ik_joints)
print('joint lower limit: {}'.format(lower_limits))
print('joint upper limit: {}'.format(upper_limits))
# we can also read these velocities from the SRDF file (using e.g. COMPAS_FAB)
# I'm a bit lazy, just handcode the values here
vel_limits = {0 : 6.28318530718,
1 : 5.23598775598,
2 : 6.28318530718,
3 : 6.6497044501,
4 : 6.77187749774,
5 : 10.7337748998}
# set the robot to a "comfortable" start configuration, optional
robot_start_conf = [0,-np.pi/2,np.pi/2,0,0,0]
set_joint_positions(robot, ik_joints, robot_start_conf)
tool_link = link_from_name(robot, EE_LINK_NAME)
robot_base_link = link_from_name(robot, ROBOT_BASE_LINK_NAME)
# tool_from_root = get_relative_pose(robot, root_link, tool_link)
# * draw EE pose
if has_gui() :
tcp_pose = get_link_pose(robot, tool_link)
draw_pose(tcp_pose)
circle_center = np.array([0.6, 0, 0.2])
circle_r = 0.1
# * generate multiple circles
ee_poses = []
full_angle = 2*2*np.pi
# total num of path pts, one path point per 5 degree
n_pt = int(full_angle / (np.pi/180 * 5))
# full_angle = np.pi
for a in np.linspace(0.0, full_angle, num=n_pt):
pt = circle_center + circle_r*np.array([np.cos(a), np.sin(a), 0])
circ_pose = multiply(Pose(point=pt, euler=Euler(yaw=a+np.pi/2)), Pose(euler=Euler(roll=np.pi*3/4)))
draw_pose(circ_pose, length=0.01)
ee_poses.append(circ_pose)
# def get_ee_sample_fn(roll_gen, pitch_gen, yaw_gen):
# def ee_sample_fn(ee_pose):
# # a finite generator
# for roll, pitch, yaw in product(roll_gen, pitch_gen, yaw_gen):
# yield multiply(ee_pose, Pose(euler=Euler(roll=roll, pitch=pitch, yaw=yaw)))
# return ee_sample_fn
# roll_sample_size = 3
# pitch_sample_size = 3
# yaw_sample_size = 1
# delta_roll = np.pi/12
# delta_pitch = np.pi/12
# 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(0.0, 2*np.pi, num=yaw_sample_size)
# ee_sample_fn = get_ee_sample_fn(roll_gen, pitch_gen, yaw_gen)
# for ep in ee_sample_fn(ee_poses[0]):
# draw_pose(ep, length=0.03)
# * baseline, keeping the EE z axis rotational dof fixed
st_time = time.time()
path = plan_cartesian_motion(robot, robot_base_link, tool_link, ee_poses)
print('Solving time: {}'.format(elapsed_time(st_time)))
if path is None:
cprint('Gradient-based ik cartesian planning cannot find a plan!', 'red')
else:
cprint('Gradient-based ik cartesian planning find a plan!', 'green')
time_step = 0.03
for conf in path:
set_joint_positions(robot, ik_joints, conf)
wait_for_duration(time_step)
if draw_graph:
print('drawing graph...')
plot_joint_val(path, lower_limits, upper_limits, method='GradIK')
print('='*20)
# * Now, let's try if using ladder graph without releasing the ee dof can give us a good trajectory
# you should be doing something similar in your picture, right?
# First, we will need ikfast to obtain ik solution variance, same in Descartes
ik_fn = ikfast_kuka_kr6_r900.get_ik
# we have to specify ik fn wrapper and feed it into pychoreo
def get_sample_ik_fn(robot, ik_fn, robot_base_link, ik_joints, 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_joints, world_from_tcp, robot_base_link, get_all=True)
return sample_ik_fn
# ik generation function stays the same for all cartesian processes
sample_ik_fn = get_sample_ik_fn(robot, ik_fn, robot_base_link, ik_joints)
# we ignore self collision in this tutorial, the collision_fn only considers joint limit now
# See : https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py
# for more info on examples on using collision function
collision_fn = get_collision_fn(robot, ik_joints, obstacles=[],
attachments=[], self_collisions=False,
# disabled_collisions=disabled_collisions,
# extra_disabled_collisions=extra_disabled_collisions,
custom_limits={})
# Let's check if our ik sampler is working properly
# uncomment the following if you want to play with this
# for p in ee_poses:
# print('-'*5)
# pb_q = inverse_kinematics(robot, tool_link, p)
# if pb_q is None:
# cprint('pb ik can\'t find an ik solution', 'red')
# qs = sample_ik_fn(p)
# if qs is not None:
# cprint('But Ikfast does find one! {}'.format(qs[0]), 'green')
# # set_joint_positions(robot, ik_joints, qs[0])
# # wait_if_gui()
# else:
# cprint('ikfast can\'t find an ik solution', 'red')
# * Ok, now we have the ik sampler and collision function ready, let's see if we can find a valid cartesian trajectory!
ee_vel = 0.005 # m/s
# st_time = time.time()
# path, cost = plan_cartesian_motion_lg(robot, ik_joints, ee_poses, sample_ik_fn, collision_fn, \
# custom_vel_limits=vel_limits, ee_vel=ee_vel)
# print('Solving time: {}'.format(elapsed_time(st_time)))
# if path is None:
# cprint('ladder graph (w/o releasing dof) cartesian planning cannot find a plan!', 'red')
# else:
# cprint('ladder graph (w/o releasing dof) cartesian planning find a plan!', 'green')
# cprint('Cost: {}'.format(cost), 'yellow')
# time_step = 0.03
# for conf in path:
# # cprint('conf: {}'.format(conf))
# set_joint_positions(robot, ik_joints, conf)
# wait_for_duration(time_step)
# if draw_graph:
# print('drawing graph...')
# plot_joint_val(path, lower_limits, upper_limits, method='IKFast+LadderGraph')
# print('='*20)
# * Now, let's see if releasing EE z axis dof can bring down the cost
# First, let's build an end effector pose sampler to release the EE z axis dof!
## * EE z axis dof generator
def get_ee_sample_fn(roll_gen, pitch_gen, yaw_gen):
def ee_sample_fn(ee_pose):
# a finite generator
for roll, pitch, yaw in product(roll_gen, pitch_gen, yaw_gen):
yield multiply(ee_pose, Pose(euler=Euler(roll=roll, pitch=pitch, yaw=yaw)))
return ee_sample_fn
# by increasing this number we can see the cost go down
roll_sample_size = 10
pitch_sample_size = 10
yaw_sample_size = 10
delta_roll = np.pi/6
delta_pitch = np.pi/6
delta_yaw = np.pi/6
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)
ee_sample_fn = get_ee_sample_fn(roll_gen, pitch_gen, yaw_gen)
st_time = time.time()
path, cost = plan_cartesian_motion_lg(robot, ik_joints, ee_poses, sample_ik_fn, collision_fn, sample_ee_fn=ee_sample_fn, \
custom_vel_limits=vel_limits, ee_vel=ee_vel)
print('Solving time: {}'.format(elapsed_time(st_time)))
# TODO: plot ee z axis deviation plot
# the ladder graph cost is just summation of all adjacent joint difference
# so the following assertion should be true
# conf_array = np.array(path)
# conf_diff = np.abs(conf_array[:-1,:] - conf_array[1:,:])
# np_cost = np.sum(conf_diff)
# assert np.allclose(np_cost, cost), '{} - {}'.format(np_cost, cost)
if path is None:
cprint('ladder graph (releasing EE z dof) cartesian planning cannot find a plan!', 'red')
else:
cprint('ladder graph (releasing EE z dof) cartesian planning find a plan!', 'cyan')
cprint('Cost: {}'.format(cost), 'yellow')
time_step = 0.03
for conf in path:
# cprint('conf: {}'.format(conf))
set_joint_positions(robot, ik_joints, conf)
wait_for_duration(time_step)
if draw_graph:
print('drawing graph...')
plot_joint_val(path, lower_limits, upper_limits, method='IKFast+LadderGraph+release z dof-res{}'.format(yaw_sample_size))
print('='*20)
# Now, let's do a scaling test:
# if scaling_test:
# res_sc = list(range(4,30))
# cost_sc = []
# for res in res_sc:
# yaw_sample_size = res
# yaw_gen = np.linspace(0.0, 2*np.pi, num=yaw_sample_size)
# ee_sample_fn = get_ee_sample_fn(yaw_gen)
# path, cost = plan_cartesian_motion_lg(robot, ik_joints, ee_poses, sample_ik_fn, collision_fn, sample_ee_fn=ee_sample_fn)
# assert path is not None
# cost_sc.append(cost)
# if res % 5 == 0:
# if draw_graph:
# plot_joint_val(path, lower_limits, upper_limits, method='IKFast+LadderGraph+release z dof-res{}'.format(yaw_sample_size))
# fig, ax = plt.subplots()
# ax.plot(res_sc, cost_sc)
# ax.set(xlabel='yaw angle discr resolution', ylabel='total joint difference cost',
# title='Total joint diff scaling test')
# ax.grid()
# fig.savefig(os.path.join(HERE, 'images', 'tota_diff-scaling.png'))
# # plt.show()
wait_if_gui('Press enter to exit')
def ur_demo(viewer=True, robot_path=UR_ROBOT_URDF, ee_path=EE_PATH, \
workspace_path=MIT_WORKSPACE_PATH, attach_obj_path=ATTACH_OBJ_PATH, obstacle_obj_path=OBSTACLE_OBJ_PATH):
# * this will create the pybullet GUI
# setting viewers=False will enter GUI-free mode
connect(use_gui=viewer)
cprint(
"Welcome to pybullet! <Ctrl+left mouse> to rotate, <Ctrl+middle mouse> to move the camera, <Ctrl+right mouse> to zoom",
'green')
cprint('But the space is empty, let\'s load our robot!', 'yellow')
# wait_for_user is your friend! It will pause the console, but having a separate thread keeping
# the GUI running so you can rotate and see
wait_for_user()
# * This is how we load a robot from a URDF, a workspace from a URDF, or simply a mesh object from an obj file
# Notice that the pybullet uses *METER* by default, make sure you scale things properly!
robot = load_pybullet(robot_path, fixed_base=True)
workspace = load_pybullet(workspace_path, fixed_base=True)
ee_body = create_obj(ee_path)
# this will print all the bodies' information in your console
dump_world()
cprint(
'You just loaded a robot, a workspace (with many static objects as its link, I modeled our good old MIT 3-412 shop here), '
+ 'and an end effector (it\'s inside the robot base now)', 'green')
wait_for_user()
# * adjust camera pose (optional)
# has_gui checks if the GUI mode is enabled
if has_gui():
camera_base_pt = (0, 0, 0)
camera_pt = np.array(camera_base_pt) + np.array([1, -0.5, 0.5])
set_camera_pose(tuple(camera_pt), camera_base_pt)
# * each joint of the robot are assigned with an integer in pybullet
ik_joints = get_movable_joints(robot)
ik_joint_names = get_joint_names(robot, ik_joints)
cprint('Joint {} \ncorresponds to:\n{}'.format(ik_joints, ik_joint_names),
'green')
robot_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0]
cprint("This is before updating pose", 'yellow')
wait_for_user()
# * set joint configuration, the robot's pose will be updated
set_joint_positions(robot, ik_joints, robot_start_conf)
cprint("This is after set joint pose: {}".format(robot_start_conf),
'green')
wait_for_user()
tool_attach_link_name = 'ee_link'
tool_attach_link = link_from_name(robot, tool_attach_link_name)
# * attach the end effector
ee_link_pose = get_link_pose(robot, tool_attach_link)
set_pose(ee_body, ee_link_pose)
ee_attach = create_attachment(robot, tool_attach_link, ee_body)
# we need to call "assign()" to update the attachment to the current end effector pose
ee_attach.assign()
# let's load a bar element (obj) and a box (pybullet primitive shape) into the world
attached_bar_body = create_obj(attach_obj_path)
box_body = create_obj(obstacle_obj_path)
cprint('We loaded a box to our scene!', 'green')
wait_for_user()
# * attach the bar
ee_link_from_tcp = Pose(point=(0.094, 0, 0))
set_pose(attached_bar_body, multiply(ee_link_pose, ee_link_from_tcp))
bar_attach = create_attachment(robot, tool_attach_link, attached_bar_body)
bar_attach.assign()
cprint('The bar element is attached to the robot', 'green')
wait_for_user()
attachments = [ee_attach, bar_attach]
# * Let's do some collision checking
# * specify disabled link pairs for collision checking (because they are adjacent / impossible to collide)
# link name corresponds to the ones specified in the URDF
# again, each robot link is assigned with an integer index in pybullet
robot_self_collision_disabled_link_names = [
('base_link', 'shoulder_link'), ('ee_link', 'wrist_1_link'),
('ee_link', 'wrist_2_link'), ('ee_link', 'wrist_3_link'),
('forearm_link', 'upper_arm_link'), ('forearm_link', 'wrist_1_link'),
('shoulder_link', 'upper_arm_link'), ('wrist_1_link', 'wrist_2_link'),
('wrist_1_link', 'wrist_3_link'), ('wrist_2_link', 'wrist_3_link')
]
self_collision_links = get_disabled_collisions(
robot, robot_self_collision_disabled_link_names)
cprint('self_collision_links disabled: {}'.format(self_collision_links),
'yellow')
extra_disabled_link_names = [('base_link', 'MIT_3412_robot_base_plate'),
('shoulder_link', 'MIT_3412_robot_base_plate')
]
extra_disabled_collisions = get_body_body_disabled_collisions(
robot, workspace, extra_disabled_link_names)
cprint('extra disabled: {}'.format(extra_disabled_collisions), 'yellow')
print('#' * 10)
cprint('Checking robot links self-collision', 'green')
collision_fn = get_collision_fn(robot,
ik_joints,
obstacles=[],
attachments=attachments,
self_collisions=True,
disabled_collisions=self_collision_links)
conf = [-1.029744, -1.623156, 2.844887, -0.977384, 1.58825, 0.314159]
# self collision, this should return true
# this function will first set the robot's joint configuration (together with the attachment)
# and check collision
# notice that turning on the diagnosis flag here will show you where the collision is happening
cprint(
'Notice that the diagnosis mode will zoom the camera to where the collision is detected. Move the camera around if you can\'t see what\'s going on there.',
'yellow')
assert collision_fn(conf, diagnosis=True)
print('#' * 10)
cprint('Checking robot links - holding attachment self-collision', 'green')
collision_fn = get_collision_fn(
robot,
ik_joints,
obstacles=[],
attachments=attachments,
self_collisions=True,
disabled_collisions=self_collision_links,
extra_disabled_collisions=extra_disabled_collisions)
conf = [0.03500, -2.26900, 2.44300, 1.117, 1.6579, 0.105]
assert collision_fn(conf, diagnosis=True)
print('\n')
print('#' * 10)
cprint('Checking robot links to obstacles (w/o links) collision', 'green')
collision_fn = get_collision_fn(
robot,
ik_joints,
obstacles=[box_body],
attachments=attachments,
self_collisions=True,
disabled_collisions=self_collision_links,
extra_disabled_collisions=extra_disabled_collisions)
conf = [
-0.105, -0.76800000000000002, 1.292, -0.61099999999999999, 1.484, 0.105
]
assert collision_fn(conf, diagnosis=True)
print('\n')
print('#' * 10)
cprint('Checking robot links to multi-link obstacle collision', 'green')
collision_fn = get_collision_fn(
robot,
ik_joints,
obstacles=[workspace],
attachments=[],
self_collisions=True,
disabled_collisions=self_collision_links,
extra_disabled_collisions=extra_disabled_collisions)
conf = [
-0.17499999999999999, -3.194, 0.33200000000000002, -1.6579999999999999,
1.431, 0.105
]
assert collision_fn(conf, diagnosis=True)
print('\n')
print('#' * 10)
cprint('Checking attachment to obstacles (w/o links) collision', 'green')
collision_fn = get_collision_fn(
robot,
ik_joints,
obstacles=[workspace, box_body],
attachments=attachments,
self_collisions=True,
disabled_collisions=self_collision_links,
extra_disabled_collisions=extra_disabled_collisions)
conf = [
-2.8100000000000001, -1.484, -1.9199999999999999, -1.6579999999999999,
1.431, 0.105
]
assert collision_fn(conf, diagnosis=True)
print('\n')
print('#' * 10)
cprint('Checking attachment to multi-link obstacle collision', 'green')
collision_fn = get_collision_fn(
robot,
ik_joints,
obstacles=[workspace],
attachments=attachments,
self_collisions=True,
disabled_collisions=self_collision_links,
extra_disabled_collisions=extra_disabled_collisions)
conf = [
-0.17499999999999999, -2.4780000000000002, 0.33200000000000002,
-1.6579999999999999, 1.431, 0.105
]
assert collision_fn(conf, diagnosis=True)
print('\n')
# * collision checking exoneration
print('#' * 10)
print('self-link collision disable')
collision_fn = get_collision_fn(robot,
ik_joints,
obstacles=[],
attachments=[],
self_collisions=False)
conf = [-1.029744, -1.623156, 2.844887, -0.977384, 1.58825, 0.314159]
assert not collision_fn(conf, diagnosis=True)
print('\n')
print('#' * 10)
cprint('robot links to obstacle collision exoneration', 'green')
cprint(
'In this example, the first collision function will check collision between the robot and the box, '
+ 'but the second collision function will ignore it.', 'yellow')
collision_fn = get_collision_fn(
robot,
ik_joints,
obstacles=[box_body],
attachments=[],
self_collisions=True,
disabled_collisions=self_collision_links,
)
collision_fn_disable = get_collision_fn(
robot,
ik_joints,
obstacles=[box_body],
attachments=[],
self_collisions=True,
disabled_collisions=self_collision_links,
extra_disabled_collisions=extra_disabled_collisions.union([
((robot, link_from_name(robot,
'forearm_link')), (box_body, BASE_LINK))
]),
)
conf = [
-3.2639999999999998, -2.6880000000000002, -0.85499999999999998, -1.536,
3.0369999999999999, -0.070000000000000007
]
assert collision_fn(conf, diagnosis=True)
assert not collision_fn_disable(conf, diagnosis=True)
print('\n')
# * joint value overflow checking & exoneration
cprint('joint value overflow checking & exoneration', 'green')
cprint(
'collision_fn also checks for robot\'s joint limit as well. We can also exonerate it by passing custom_limits into the collision_fn',
'yellow')
def get_custom_limits_from_name(robot, joint_limits):
return {
joint_from_name(robot, joint): limits
for joint, limits in joint_limits.items()
}
custom_limits = get_custom_limits_from_name(robot, {
'shoulder_pan_joint': (-7.9, 0),
'elbow_joint': (-8.0, 0)
})
collision_fn = get_collision_fn(robot, ik_joints)
collision_fn_disable = get_collision_fn(robot,
ik_joints,
custom_limits=custom_limits)
conf = [
-7.8450000000000002, -2.1469999999999998, -7.99, -0.92500000000000004,
1.78, 0.105
]
assert collision_fn(conf, diagnosis=True)
assert not collision_fn_disable(conf, diagnosis=True)
print('\n')
def add_attached_collision_mesh(self,
attached_collision_mesh,
options=None):
"""Adds an attached collision object to the planning scene, the grasp pose is set according to the
**current** relative pose in the scene. Thus, the robot conf needs to be set to the right values to
make the grasp pose right.
Note: the pybullet fixed constraint only affects physics simulation by adding an artificial force,
Thus, by `set_joint_configuration` and `step_simulation`, the attached object will not move together.
Thus, we use `pybullet_planning`'s `Attachment` class to simplify kinematics.
Parameters
----------
attached_collision_mesh : :class:`compas_fab.robots.AttachedCollisionMesh`
Returns
-------
attachment : a pybullet_planning `Attachment` object
"""
options = options or {}
assert 'robot' in options, 'The robot option must be specified!'
robot = options['robot']
mass = options.get('mass', STATIC_MASS)
options['mass'] = mass
color = options.get('color', None)
attached_child_link_name = options.get('attached_child_link_name',
None)
robot_uid = robot.attributes['pybullet_uid']
name = attached_collision_mesh.collision_mesh.id
attached_bodies = []
# ! mimic ROS' behavior: collision object with same name is replaced
if name in self.attached_collision_objects:
cprint(
'Replacing existing attached collision mesh {}'.format(name),
'yellow')
self.remove_attached_collision_mesh(name)
if name not in self.collision_objects:
# ! I don't want to add another copy of the objects
# self.planner.add_attached_collision_mesh(attached_collision_mesh, options=options)
# attached_bodies = [constr.body_id for constr in self.attached_collision_objects[name]]
self.planner.add_collision_mesh(
attached_collision_mesh.collision_mesh, options=options)
attached_bodies = self.collision_objects[name]
del self.collision_objects[name]
self.attached_collision_objects[name] = []
tool_attach_link = link_from_name(robot_uid,
attached_collision_mesh.link_name)
for body in attached_bodies:
# TODO let user choose to include the direct touch link collision or not
# * update attachment collision links
for touched_link_name in attached_collision_mesh.touch_links:
self.extra_disabled_collision_links[name].add(
((robot_uid, touched_link_name), (body, None)))
links = get_all_links(body)
if color:
for link in links:
set_color(body, color, link=link)
# create attachment based on their *current* pose
attach_child_link = BASE_LINK if not attached_child_link_name else link_from_name(
body, attached_child_link_name)
attachment = create_attachment(robot_uid, tool_attach_link, body,
attach_child_link)
attachment.assign()
self.pychoreo_attachments[name].append(attachment)
# create fixed constraint to conform to PybulletClient (we don't use it though)
constraint_id = add_fixed_constraint(attachment.child,
attachment.parent,
attachment.parent_link)
constraint_info = ConstraintInfo(constraint_id, attachment.child,
attachment.parent)
self.attached_collision_objects[name].append(constraint_info)
return self.pychoreo_attachments[name]
def get_link_frame_from_name(self, robot, link_name):
robot_uid = robot.attributes['pybullet_uid']
return frame_from_pose(
get_link_pose(robot_uid, link_from_name(robot_uid, link_name)))
def tf_world_frame(self, base_to_pose, armname):
basename = self.arms_base[armname]
baseid = pyplan.link_from_name(self.id, basename)
world_to_base = pyplan.get_link_pose(self.id, baseid)
world_to_pose = pyplan.multiply(world_to_base, base_to_pose)
return world_to_pose
def tf_arm_frame(self, pose, armname):
basename = self.arms_base[armname]
baseid = pyplan.link_from_name(self.id, basename)
base_to_world = pyplan.invert(pyplan.get_link_pose(self.id, baseid))
base_to_pose = pyplan.multiply(base_to_world, pose)
return base_to_pose
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()