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