def cube_tip_collision_fn(cube_pose, joint_conf, diagnosis=diagnosis):
cube_pos = cube_pose[:3]
cube_quat = p.getQuaternionFromEuler(cube_pose[3:])
if vis_fn is not None:
vis_fn(cube_pos, cube_quat)
set_joint_positions(finger_body, finger_joints, joint_conf)
return collision_fn((cube_pos, cube_quat), diagnosis)
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
# dummy joints
if all(joint < -1 for joint in joints):
cube_pos = q[:3]
cube_ori = q[3:]
cube_quat = p.getQuaternionFromEuler(cube_ori)
set_pose(body, (cube_pos, cube_quat))
else:
# * 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, **kwargs):
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
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
def birrt(q1,
q2,
distance_fn,
sample_fn,
extend_fn,
collision_fn,
restarts=RRT_RESTARTS,
smooth=RRT_SMOOTHING,
max_time=INF,
**kwargs):
"""birrt [summary]
TODO: add citation to the algorithm.
See `pybullet_planning.interfaces.planner_interface.joint_motion_planning.plan_joint_motion` for an example
of standard usage.
Parameters
----------
q1 : [type]
[description]
q2 : [type]
[description]
distance_fn : [type]
see `pybullet_planning.interfaces.planner_interface.joint_motion_planning.get_difference_fn` for an example
sample_fn : function handle
configuration space sampler
see `pybullet_planning.interfaces.planner_interface.joint_motion_planning.get_sample_fn` for an example
extend_fn : function handle
see `pybullet_planning.interfaces.planner_interface.joint_motion_planning.get_extend_fn` for an example
collision_fn : function handle
collision checking function
see `pybullet_planning.interfaces.robots.collision.get_collision_fn` for an example
restarts : int, optional
[description], by default RRT_RESTARTS
iterations : int, optional
[description], by default RRT_ITERATIONS
smooth : int, optional
smoothing iterations, by default RRT_SMOOTHING
Returns
-------
[type]
[description]
"""
start_time = time.time()
if collision_fn(q1) or collision_fn(q2):
return None, None
path, dis = direct_path(q1, q2, extend_fn, collision_fn)
if path is not None:
return path, dis
for _ in irange(restarts + 1):
if max_time <= elapsed_time(start_time):
break
path = rrt_connect(q1,
q2,
distance_fn,
sample_fn,
extend_fn,
collision_fn,
max_time=max_time - elapsed_time(start_time),
**kwargs)
dis = []
if path is not None:
if smooth is None:
path = path
else:
path = smooth_path(path,
extend_fn,
collision_fn,
iterations=smooth)
for q in path:
set_joint_positions(BODY1, joints, q)
dis_tmp = p.getClosestPoints(bodyA=1,
bodyB=2,
distance=100000,
physicsClientId=CLIENT)
dis_tmp = (np.array(dis_tmp))[:, 8]
dis_tmp = np.min(dis_tmp)
dis.append(dis_tmp)
return path, dis
# return smooth_path(path, extend_fn, collision_fn, iterations=smooth)
return None, None
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 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
