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)
