def get_moving_links(body, joints):
from pybullet_planning.interfaces.robots.link import child_link_from_joint
from pybullet_planning.interfaces.robots.link import get_link_subtree
moving_links = set()
for joint in joints:
link = child_link_from_joint(joint)
if link not in moving_links:
moving_links.update(get_link_subtree(body, link))
return list(moving_links)
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 bodies(self):
from pybullet_planning.interfaces.robots.collision import flatten_links
return flatten_links(self.child) | flatten_links(
self.parent, get_link_subtree(self.parent, self.parent_link))
def get_subtree_aabb(body, root_link=BASE_LINK):
from pybullet_planning.interfaces.robots.link import get_link_subtree
return aabb_union(get_aabb(body, link) for link in get_link_subtree(body, root_link))
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