def inverse_kinematics(robot,
link,
target_pose,
max_iterations=200,
custom_limits={},
**kwargs):
from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
from pybullet_planning.interfaces.robots import get_movable_joints, set_joint_positions, get_link_pose, get_custom_limits
movable_joints = get_movable_joints(robot)
for iterations in range(max_iterations):
# TODO: stop is no progress
# TODO: stop if collision or invalid joint limits
kinematic_conf = inverse_kinematics_helper(robot, link, target_pose)
if kinematic_conf is None:
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
if is_pose_close(get_link_pose(robot, link), target_pose, **kwargs):
break
else:
return None
lower_limits, upper_limits = get_custom_limits(robot, movable_joints,
custom_limits)
if not all_between(lower_limits, kinematic_conf, upper_limits):
return None
return kinematic_conf
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 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
