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 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_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 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_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): """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 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 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 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 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 is_point_on_surface(polygon, world_from_surface, point_world): [point_surface] = apply_affine(invert(world_from_surface), [point_world]) return is_point_in_polygon(point_surface, polygon[::-1])