def get_ik_generator(ik_fn, robot, base_link, world_from_tcp, ik_tool_link_from_tcp=None, **kwargs): """get an ik generator Parameters ---------- ik_fn : function handle get_ik(point, rot) : list of jt solutions point = [x,y,z] rot = 3x3 rotational matrix as a row-major list robot : pybullet robot base_link : int robot base link index, can be obtained from ``link_from_name(robot, base_link_name)`` world_from_tcp : pybullet pose tcp pose in the world frame ik_tool_link_from_tcp : pybullet pose tcp pose in ik tool link's frame, optional, defaults = None Returns ------- generator use next() to get solutions """ world_from_base = get_link_pose(robot, base_link) base_from_tcp = multiply(invert(world_from_base), world_from_tcp) if ik_tool_link_from_tcp: base_from_ik_tool_link = multiply(base_from_tcp, invert(ik_tool_link_from_tcp)) else: base_from_ik_tool_link = base_from_tcp yield compute_inverse_kinematics(ik_fn, base_from_ik_tool_link, **kwargs)
def sample_edge_pose(polygon, world_from_surface, mesh): radius = max(get_length(v[:2]) for v in mesh.vertices) origin_from_base = Pose(Point(z=p.min(mesh.vertices[:, 2]))) for point in sample_edge_point(polygon, radius): theta = np.random.uniform(0, 2 * np.pi) surface_from_origin = Pose(point, Euler(yaw=theta)) yield multiply(world_from_surface, surface_from_origin, origin_from_base)
def end_effector_from_body(body_pose, grasp_pose): """ grasp_pose: the body's pose in gripper's frame world_from_child * (parent_from_child)^(-1) = world_from_parent (parent: gripper, child: body to be grasped) Pose_{world,gripper} = Pose_{world,block}*Pose_{block,gripper} = Pose_{world,block}*(Pose_{gripper,block})^{-1} """ return multiply(body_pose, invert(grasp_pose))
def is_mesh_on_surface(polygon, world_from_surface, mesh, world_from_mesh, epsilon=1e-2): surface_from_mesh = multiply(invert(world_from_surface), world_from_mesh) points_surface = apply_affine(surface_from_mesh, mesh.vertices) min_z = np.min(points_surface[:, 2]) return (abs(min_z) < epsilon) and \ all(is_point_in_polygon(p, polygon) for p in points_surface)
def get_local_link_pose(body, joint): from pybullet_planning.interfaces.env_manager.pose_transformation import Pose, multiply, invert from pybullet_planning.interfaces.robots.joint import get_joint_parent_frame from pybullet_planning.interfaces.robots.link import parent_link_from_joint parent_joint = parent_link_from_joint(body, joint) #world_child = get_link_pose(body, joint) #world_parent = get_link_pose(body, parent_joint) ##return multiply(invert(world_parent), world_child) #return multiply(world_child, invert(world_parent)) # https://github.com/bulletphysics/bullet3/blob/9c9ac6cba8118544808889664326fd6f06d9eeba/examples/pybullet/gym/pybullet_utils/urdfEditor.py#L169 parent_com = get_joint_parent_frame(body, joint) tmp_pose = invert( multiply(get_joint_inertial_pose(body, joint), parent_com)) parent_inertia = get_joint_inertial_pose(body, parent_joint) #return multiply(parent_inertia, tmp_pose) # TODO: why is this wrong... _, orn = multiply(parent_inertia, tmp_pose) pos, _ = multiply(parent_inertia, Pose(parent_com[0])) return (pos, orn)
def get_grasp_pose(constraint): """ Grasps are parent_from_child """ from pybullet_planning.interfaces.task_modeling.constraint import get_constraint_info constraint_info = get_constraint_info(constraint) assert (constraint_info.constraintType == p.JOINT_FIXED) joint_from_parent = (constraint_info.jointPivotInParent, constraint_info.jointFrameOrientationParent) joint_from_child = (constraint_info.jointPivotInChild, constraint_info.jointFrameOrientationChild) return multiply(invert(joint_from_parent), joint_from_child)
def sample_placement_on_aabb(top_body, bottom_aabb, top_pose=unit_pose(), percent=1.0, max_attempts=50, epsilon=1e-3): # TODO: transform into the coordinate system of the bottom # TODO: maybe I should instead just require that already in correct frame for _ in range(max_attempts): theta = np.random.uniform(*CIRCULAR_LIMITS) rotation = Euler(yaw=theta) set_pose(top_body, multiply(Pose(euler=rotation), top_pose)) center, extent = get_center_extent(top_body) lower = (np.array(bottom_aabb[0]) + percent * extent / 2)[:2] upper = (np.array(bottom_aabb[1]) - percent * extent / 2)[:2] if np.less(upper, lower).any(): continue x, y = np.random.uniform(lower, upper) z = (bottom_aabb[1] + extent / 2.)[2] + epsilon point = np.array([x, y, z]) + (get_point(top_body) - center) pose = multiply(Pose(point, rotation), top_pose) set_pose(top_body, pose) return pose return None
def approach_from_grasp(approach_pose, end_effector_pose): """get approach_from_brick Parameters ---------- approach_pose : [type] approach_from_gripper end_effector_pose : [type] gripper_from_brick Returns ------- [type] approach_from_brick """ return multiply(approach_pose, end_effector_pose)
def body_from_end_effector(end_effector_pose, grasp_pose): """get world_from_brick pose from a given world_from_gripper and gripper_from_brick Parameters ---------- end_effector_pose : [type] world_from_gripper grasp_pose : [type] gripper_from_brick Returns ------- Pose world_from_brick """ return multiply(end_effector_pose, grasp_pose)
def create_attachment(parent, parent_link, child): """create an Attachment between the parent body's parent_link and child body, based on their **current pose** Parameters ---------- parent : int parent body's pb index parent_link : int parent body's attach link index child : [type] child body's pb index Returns ------- Attachment """ parent_link_pose = get_link_pose(parent, parent_link) child_pose = get_pose(child) grasp_pose = multiply(invert(parent_link_pose), child_pose) return Attachment(parent, parent_link, grasp_pose, child)
def get_grasp_pose(constraint): """get grasp poses from a constraint Parameters ---------- constraint : [type] [description] Returns ------- [type] [description] """ from pybullet_planning.interfaces.task_modeling.constraint import get_constraint_info constraint_info = get_constraint_info(constraint) assert (constraint_info.constraintType == p.JOINT_FIXED) joint_from_parent = (constraint_info.jointPivotInParent, constraint_info.jointFrameOrientationParent) joint_from_child = (constraint_info.jointPivotInChild, constraint_info.jointFrameOrientationChild) return multiply(invert(joint_from_parent), joint_from_child)
def end_effector_from_body(body_pose, grasp_pose): """get world_from_gripper from the target brick's pose and the grasp pose world_from_child * (parent_from_child)^(-1) = world_from_parent (parent: gripper, child: body to be grasped) world_from_gripper = world_from_block * block_from_gripper = world_from_block * invert(gripper_from_block) Parameters ---------- body_pose : Pose world_from_body grasp_pose : Pose gripper_from_body, the body's pose in gripper's frame Returns ------- Pose world_from_gripper """ return multiply(body_pose, invert(grasp_pose))
def collision_shape_from_data(data, body, link, client=None): from pybullet_planning.interfaces.env_manager.pose_transformation import multiply from pybullet_planning.interfaces.robots.dynamics import get_joint_inertial_pose client = get_client(client) if (data.geometry_type == p.GEOM_MESH) and (data.filename == UNKNOWN_FILE): return NULL_ID pose = multiply(get_joint_inertial_pose(body, link), get_data_pose(data)) point, quat = pose # TODO: the visual data seems affected by the collision data return p.createCollisionShape(shapeType=data.geometry_type, radius=get_data_radius(data), # halfExtents=get_data_extents(data.geometry_type, data.dimensions), halfExtents=np.array(get_data_extents(data)) / 2, height=get_data_height(data), fileName=data.filename.decode(encoding='UTF-8'), meshScale=get_data_scale(data), planeNormal=get_data_normal(data), flags=p.GEOM_FORCE_CONCAVE_TRIMESH, collisionFramePosition=point, collisionFrameOrientation=quat, physicsClientId=client)
def get_relative_pose(body, link1, link2=BASE_LINK): """compute relative transformation between two links of a body Parameters ---------- body : [type] [description] link1 : [type] [description] link2 : [type], optional [description], by default BASE_LINK Returns ------- [type] [description] """ from pybullet_planning.interfaces.env_manager.pose_transformation import multiply, invert world_from_link1 = get_link_pose(body, link1) world_from_link2 = get_link_pose(body, link2) link2_from_link1 = multiply(invert(world_from_link2), world_from_link1) return link2_from_link1
def get_ik_tool_link_pose(fk_fn, robot, ik_joints, base_link, \ joint_values=None, use_current=False): """Use the given forward_kinematics function to compute ik_tool_link pose based on current joint configurations in pybullet. Note: The resulting FK pose is relative to the world frame, not the robot's base frame. Parameters ---------- fk_fn : function handle fk(a 6-list) : point, rot_matrix robot : pybullet robot ik_joint : list of int a list of pybullet joint index that is registered for IK/FK can be obtained by e.g.: ``ik_joints = joints_from_names(robot, ik_joint_names)`` base_link : int robot base link index, can be obtained from ``link_from_name(robot, base_link_name)`` joint_values : list of float robot joint values for FK computation, value correponds to ik_joint_names, default to None use_current: bool if true, use the current configuration in pybullet env for FK, default to False Returns ------- pybullet Pose Pose = (point, quat) = ([x,y,z], [4-list]) """ if use_current: conf = get_joint_positions(robot, ik_joints) else: assert joint_values conf = joint_values base_from_tool = compute_forward_kinematics(fk_fn, conf) world_from_base = get_link_pose(robot, base_link) return multiply(world_from_base, base_from_tool)
def add_fixed_constraint(body, robot, robot_link, max_force=None): from pybullet_planning.interfaces.env_manager.pose_transformation import get_pose, unit_point, unit_quat, multiply, invert from pybullet_planning.interfaces.robots import get_com_pose body_link = BASE_LINK body_pose = get_pose(body) #body_pose = get_com_pose(body, link=body_link) #end_effector_pose = get_link_pose(robot, robot_link) end_effector_pose = get_com_pose(robot, robot_link) grasp_pose = multiply(invert(end_effector_pose), body_pose) point, quat = grasp_pose # TODO: can I do this when I'm not adjacent? # joint axis in local frame (ignored for JOINT_FIXED) #return p.createConstraint(robot, robot_link, body, body_link, # p.JOINT_FIXED, jointAxis=unit_point(), # parentFramePosition=unit_point(), # childFramePosition=point, # parentFrameOrientation=unit_quat(), # childFrameOrientation=quat) constraint = p.createConstraint( robot, robot_link, body, body_link, # Both seem to work p.JOINT_FIXED, jointAxis=unit_point(), parentFramePosition=point, childFramePosition=unit_point(), parentFrameOrientation=quat, childFrameOrientation=unit_quat(), physicsClientId=CLIENT) if max_force is not None: p.changeConstraint(constraint, maxForce=max_force, physicsClientId=CLIENT) return constraint
def sample_surface_pose(polygon, world_from_surface, mesh): for surface_from_origin in sample_polygon_tform(polygon, mesh.vertices): world_from_mesh = multiply(world_from_surface, surface_from_origin) if is_mesh_on_surface(polygon, world_from_surface, mesh, world_from_mesh): yield world_from_mesh
def create_attachment(parent, parent_link, child): parent_link_pose = get_link_pose(parent, parent_link) child_pose = get_pose(child) grasp_pose = multiply(invert(parent_link_pose), child_pose) return Attachment(parent, parent_link, grasp_pose, child)
def get_relative_pose(body, link1, link2=BASE_LINK): from pybullet_planning.interfaces.env_manager.pose_transformation import multiply, invert world_from_link1 = get_link_pose(body, link1) world_from_link2 = get_link_pose(body, link2) link2_from_link1 = multiply(invert(world_from_link2), world_from_link1) return link2_from_link1
def body_from_end_effector(end_effector_pose, grasp_pose): """ world_from_parent * parent_from_child = world_from_child """ return multiply(end_effector_pose, grasp_pose)
def approach_from_grasp(approach_pose, end_effector_pose): return multiply(approach_pose, end_effector_pose)