def get_null_space(robot, joints, custom_limits={}):
rest_positions = get_joint_positions(robot, joints)
lower, upper = get_custom_limits(robot, joints, custom_limits)
lower = np.maximum(lower, -10 * np.ones(len(joints)))
upper = np.minimum(upper, +10 * np.ones(len(joints)))
joint_ranges = 10 * np.ones(len(joints))
return NullSpace(list(lower), list(upper), list(joint_ranges),
list(rest_positions))
def get_sample_fn(body, joints, custom_limits={}, **kwargs):
lower_limits, upper_limits = get_custom_limits(
body, joints, custom_limits, circular_limits=CIRCULAR_LIMITS)
generator = interval_generator(lower_limits, upper_limits, **kwargs)
def fn():
return tuple(next(generator))
return fn
def get_sample_fn(body, joints, custom_limits={}, **kwargs):
generator = unit_generator(len(joints), **kwargs)
lower_limits, upper_limits = get_custom_limits(
body, joints, custom_limits, circular_limits=CIRCULAR_LIMITS)
limits_extents = np.array(upper_limits) - np.array(lower_limits)
def fn():
return tuple(next(generator) * limits_extents + np.array(lower_limits))
return fn
def get_collision_fn(body,
joints,
obstacles,
attachments,
self_collisions,
disabled_collisions,
custom_limits={},
**kwargs):
from pybullet_planning.interfaces.robots.collision import pairwise_collision, pairwise_link_collision
from pybullet_planning.interfaces.robots.joint import set_joint_positions
# TODO: convert most of these to keyword arguments
check_link_pairs = get_self_link_pairs(body, joints, disabled_collisions) \
if self_collisions else []
moving_links = frozenset(get_moving_links(body, joints))
attached_bodies = [attachment.child for attachment in attachments]
moving_bodies = [(body, moving_links)] + attached_bodies
#moving_bodies = [body] + [attachment.child for attachment in attachments]
check_body_pairs = list(product(
moving_bodies, obstacles)) # + list(combinations(moving_bodies, 2))
lower_limits, upper_limits = get_custom_limits(body, joints, custom_limits)
# TODO: maybe prune the link adjacent to the robot
# TODO: test self collision with the holding
def collision_fn(q):
if not all_between(lower_limits, q, upper_limits):
#print('Joint limits violated')
return True
set_joint_positions(body, joints, q)
for attachment in attachments:
attachment.assign()
for link1, link2 in check_link_pairs:
# Self-collisions should not have the max_distance parameter
if pairwise_link_collision(body, link1, body,
link2): #, **kwargs):
#print(get_body_name(body), get_link_name(body, link1), get_link_name(body, link2))
return True
for body1, body2 in check_body_pairs:
if pairwise_collision(body1, body2, **kwargs):
#print(get_body_name(body1), get_body_name(body2))
return True
return False
return collision_fn
def plan_cartesian_motion(robot,
first_joint,
target_link,
waypoint_poses,
max_iterations=200,
custom_limits={},
get_sub_conf=False,
**kwargs):
"""Compute a joint trajectory for a given sequence of workspace poses. Only joint limit is considered.
Collision checking using `get_collision_fn` is often performed on the path computed by this function.
Parameters
----------
robot : int
robot body index
first_joint : int
the first joint index in the kinematics chain.
target_link : int
end effector link index.
waypoint_poses : a list of Pose
a list of end effector workspace poses in the world coord.
max_iterations : int, optional
[description], by default 200
custom_limits : dict, optional
[description], by default {}
get_sub_conf : bool, optional
return sub-kinematics chain configuration if set to True, by default False
Returns
-------
[type]
[description]
Example
-------
```
ik_joints = joints_from_names(robot_uid, ik_joint_names)
ik_tool_link = link_from_name(robot_uid, tool_link_name)
cart_conf_vals = plan_cartesian_motion(robot_uid, ik_joints[0], ik_tool_link, world_from_ee_poses)
```
"""
from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
from pybullet_planning.interfaces.robots.link import get_link_subtree, prune_fixed_joints
from pybullet_planning.interfaces.kinematics import inverse_kinematics_helper, is_pose_close
# TODO: fix stationary joints
# TODO: pass in set of movable joints and take least common ancestor
# TODO: update with most recent bullet updates
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics.py
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
# TODO: plan a path without needing to following intermediate waypoints
lower_limits, upper_limits = get_custom_limits(robot,
get_movable_joints(robot),
custom_limits)
selected_links = get_link_subtree(
robot, first_joint) # TODO: child_link_from_joint?
selected_movable_joints = prune_fixed_joints(robot, selected_links)
assert (target_link in selected_links)
selected_target_link = selected_links.index(target_link)
sub_robot = clone_body(robot,
links=selected_links,
visual=False,
collision=False) # TODO: joint limits
sub_movable_joints = get_movable_joints(sub_robot)
#null_space = get_null_space(robot, selected_movable_joints, custom_limits=custom_limits)
null_space = None
solutions = []
for target_pose in waypoint_poses:
for iteration in range(max_iterations):
sub_kinematic_conf = inverse_kinematics_helper(
sub_robot,
selected_target_link,
target_pose,
null_space=null_space)
if sub_kinematic_conf is None:
remove_body(sub_robot)
return None
set_joint_positions(sub_robot, sub_movable_joints,
sub_kinematic_conf)
if is_pose_close(get_link_pose(sub_robot, selected_target_link),
target_pose, **kwargs):
set_joint_positions(robot, selected_movable_joints,
sub_kinematic_conf)
kinematic_conf = get_configuration(robot)
if not all_between(lower_limits, kinematic_conf, upper_limits):
#movable_joints = get_movable_joints(robot)
#print([(get_joint_name(robot, j), l, v, u) for j, l, v, u in
# zip(movable_joints, lower_limits, kinematic_conf, upper_limits) if not (l <= v <= u)])
#print("Limits violated")
#wait_for_user()
remove_body(sub_robot)
return None
#print("IK iterations:", iteration)
if not get_sub_conf:
solutions.append(kinematic_conf)
else:
solutions.append(sub_kinematic_conf)
break
else:
remove_body(sub_robot)
return None
remove_body(sub_robot)
return solutions
def get_collision_fn(body,
joints,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions={},
extra_disabled_collisions={},
custom_limits={},
**kwargs):
"""get collision checking function collision_fn(joint_values) -> bool.
* Note: This function might be one of the most heavily used function in this suite and
is very important for planning applications. Backward compatibility (for Caelan's PDDLStream)
is definitely on top priority.
Parameters
----------
body : int
the main moving body (usually the robot). We refer to this body as 'the main body'
in this function's docstring.
joints : list of int
moving joint indices for body
obstacles : list of int
body indices for collision objects, by default []
attachments : list of Attachment, optional
list of attachment, by default []
self_collisions : bool, optional
checking self collisions between links of the body or not, by default True
disabled_collisions : set of tuples, optional
list of tuples of two integers, representing the **main body's** link indices pair that is
ignored in collision checking, by default {}
extra_disabled_collisions : set of tuples, optional
list of tuples for specifying disabled collisions, the tuple must be of the following format:
((int, int), (int, int)) : (body index, link index), (body index, link index)
If the body considered is a single-link (floating) body, assign the link index to BASE_LINK.
reversing the order of the tuples above is also acceptable, by default {}
custom_limits: dict, optional
customized joint range, example: {joint index (int) : (-np.pi/2, np.pi/2)}, by default {}
Returns
-------
function handle
collision_fn: (conf, diagnosis) -> False if no collision found, True otherwise.
if need diagnosis information for the collision, set diagnosis to True will help you visualize
which link is colliding to which.
"""
from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
from pybullet_planning.interfaces.robots.joint import set_joint_positions, get_custom_limits
from pybullet_planning.interfaces.robots.link import get_self_link_pairs, get_moving_links
from pybullet_planning.interfaces.debug_utils.debug_utils import draw_collision_diagnosis
moving_links = frozenset(get_moving_links(body, joints))
attached_bodies = [attachment.child for attachment in attachments]
moving_bodies = [(body, moving_links)] + attached_bodies
# * main body self-collision link pairs
self_check_link_pairs = get_self_link_pairs(
body, joints, disabled_collisions) if self_collisions else []
# * main body link - attachment body pairs
attach_check_pairs = []
for attached in attachments:
if attached.parent != body:
continue
# prune the main body link adjacent to the attachment and the ones in ignored collisions
# TODO: prune the link that's adjacent to the attach link as well?
# ? i.e. object attached to ee_tool_link, and ee geometry is attached to ee_base_link
at_check_links = [
ml for ml in moving_links if ml != attached.parent_link and (
(body, ml), (attached.child,
BASE_LINK)) not in extra_disabled_collisions and (
(attached.child, BASE_LINK),
(body, ml)) not in extra_disabled_collisions
]
attach_check_pairs.append((at_check_links, attached.child))
# * body pairs
check_body_pairs = list(product(
moving_bodies, obstacles)) # + list(combinations(moving_bodies, 2))
check_body_link_pairs = []
for body1, body2 in check_body_pairs:
body1, links1 = expand_links(body1)
body2, links2 = expand_links(body2)
if body1 == body2:
continue
bb_link_pairs = product(links1, links2)
for bb_links in bb_link_pairs:
bbll_pair = ((body1, bb_links[0]), (body2, bb_links[1]))
if bbll_pair not in extra_disabled_collisions and bbll_pair[::
-1] not in extra_disabled_collisions:
check_body_link_pairs.append(bbll_pair)
# print('check pairs: ', check_body_link_pairs)
# print('extra disabled: ', extra_disabled_collisions)
# * joint limits
lower_limits, upper_limits = get_custom_limits(body, joints, custom_limits)
# TODO: maybe prune the link adjacent to the robot
def collision_fn(q, diagnosis=False):
# * joint limit check
if not all_between(lower_limits, q, upper_limits):
if diagnosis:
warnings.warn('joint limit violation!', UserWarning)
cr = np.less_equal(q, lower_limits), np.less_equal(
upper_limits, q)
print('joint limit violation : {} / {}'.format(cr[0], cr[1]))
for i, (cr_l, cr_u) in enumerate(zip(cr[0], cr[1])):
if cr_l:
print('J{}: {} < lower limit {}'.format(
i, q[i], lower_limits[i]))
if cr_u:
print('J{}: {} > upper limit {}'.format(
i, q[i], upper_limits[i]))
return True
# * set body & attachment positions
set_joint_positions(body, joints, q)
for attachment in attachments:
attachment.assign()
# * self-collision link check
for link1, link2 in self_check_link_pairs:
if pairwise_link_collision(body, link1, body, link2):
if diagnosis:
warnings.warn(
'moving body link - moving body link collision!',
UserWarning)
cr = pairwise_link_collision_info(body, link1, body, link2)
draw_collision_diagnosis(cr)
return True
# * self link - attachment check
for body_check_links, attached_body in attach_check_pairs:
if any_link_pair_collision(body, body_check_links, attached_body,
**kwargs):
if diagnosis:
warnings.warn('moving body link - attachement collision!',
UserWarning)
cr = any_link_pair_collision_info(body, body_check_links,
attached_body, **kwargs)
draw_collision_diagnosis(cr)
return True
# * body - body check
for (body1, link1), (body2, link2) in check_body_link_pairs:
if pairwise_link_collision(body1, link1, body2, link2, **kwargs):
if diagnosis:
warnings.warn('moving body - body collision!', UserWarning)
cr = pairwise_link_collision_info(body1, link1, body2,
link2)
draw_collision_diagnosis(cr)
return True
return False
return collision_fn
def plan_cartesian_motion(robot,
first_joint,
target_link,
waypoint_poses,
max_iterations=200,
custom_limits={},
**kwargs):
from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
from pybullet_planning.interfaces.robots.link import get_link_subtree, prune_fixed_joints
from pybullet_planning.interfaces.kinematics import inverse_kinematics_helper, is_pose_close
# TODO: fix stationary joints
# TODO: pass in set of movable joints and take least common ancestor
# TODO: update with most recent bullet updates
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics.py
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
# TODO: plan a path without needing to following intermediate waypoints
lower_limits, upper_limits = get_custom_limits(robot,
get_movable_joints(robot),
custom_limits)
selected_links = get_link_subtree(
robot, first_joint) # TODO: child_link_from_joint?
selected_movable_joints = prune_fixed_joints(robot, selected_links)
assert (target_link in selected_links)
selected_target_link = selected_links.index(target_link)
sub_robot = clone_body(robot,
links=selected_links,
visual=False,
collision=False) # TODO: joint limits
sub_movable_joints = get_movable_joints(sub_robot)
#null_space = get_null_space(robot, selected_movable_joints, custom_limits=custom_limits)
null_space = None
solutions = []
for target_pose in waypoint_poses:
for iteration in range(max_iterations):
sub_kinematic_conf = inverse_kinematics_helper(
sub_robot,
selected_target_link,
target_pose,
null_space=null_space)
if sub_kinematic_conf is None:
remove_body(sub_robot)
return None
set_joint_positions(sub_robot, sub_movable_joints,
sub_kinematic_conf)
if is_pose_close(get_link_pose(sub_robot, selected_target_link),
target_pose, **kwargs):
set_joint_positions(robot, selected_movable_joints,
sub_kinematic_conf)
kinematic_conf = get_configuration(robot)
if not all_between(lower_limits, kinematic_conf, upper_limits):
#movable_joints = get_movable_joints(robot)
#print([(get_joint_name(robot, j), l, v, u) for j, l, v, u in
# zip(movable_joints, lower_limits, kinematic_conf, upper_limits) if not (l <= v <= u)])
#print("Limits violated")
#wait_for_user()
remove_body(sub_robot)
return None
#print("IK iterations:", iteration)
solutions.append(kinematic_conf)
break
else:
remove_body(sub_robot)
return None
remove_body(sub_robot)
return solutions
