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])
