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 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_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 assign(self):
parent_link_pose = get_link_pose(self.parent, self.parent_link)
child_pose = body_from_end_effector(parent_link_pose, self.grasp_pose)
set_pose(self.child, child_pose)
return child_pose
def clone_body(body, links=None, collision=True, visual=True, client=None):
from pybullet_planning.utils import get_client
# TODO: names are not retained
# TODO: error with createMultiBody link poses on PR2
# localVisualFrame_position: position of local visual frame, relative to link/joint frame
# localVisualFrame orientation: orientation of local visual frame relative to link/joint frame
# parentFramePos: joint position in parent frame
# parentFrameOrn: joint orientation in parent frame
client = get_client(client) # client is the new client for the body
if links is None:
links = get_links(body)
#movable_joints = [joint for joint in links if is_movable(body, joint)]
new_from_original = {}
base_link = get_link_parent(body, links[0]) if links else BASE_LINK
new_from_original[base_link] = -1
masses = []
collision_shapes = []
visual_shapes = []
positions = [] # list of local link positions, with respect to parent
orientations = [] # list of local link orientations, w.r.t. parent
inertial_positions = [] # list of local inertial frame pos. in link frame
inertial_orientations = [] # list of local inertial frame orn. in link frame
parent_indices = []
joint_types = []
joint_axes = []
for i, link in enumerate(links):
new_from_original[link] = i
joint_info = get_joint_info(body, link)
dynamics_info = get_dynamics_info(body, link)
masses.append(dynamics_info.mass)
collision_shapes.append(clone_collision_shape(body, link, client) if collision else -1)
visual_shapes.append(clone_visual_shape(body, link, client) if visual else -1)
point, quat = get_local_link_pose(body, link)
positions.append(point)
orientations.append(quat)
inertial_positions.append(dynamics_info.local_inertial_pos)
inertial_orientations.append(dynamics_info.local_inertial_orn)
parent_indices.append(new_from_original[joint_info.parentIndex] + 1) # TODO: need the increment to work
joint_types.append(joint_info.jointType)
joint_axes.append(joint_info.jointAxis)
# https://github.com/bulletphysics/bullet3/blob/9c9ac6cba8118544808889664326fd6f06d9eeba/examples/pybullet/gym/pybullet_utils/urdfEditor.py#L169
base_dynamics_info = get_dynamics_info(body, base_link)
base_point, base_quat = get_link_pose(body, base_link)
new_body = p.createMultiBody(baseMass=base_dynamics_info.mass,
baseCollisionShapeIndex=clone_collision_shape(body, base_link, client) if collision else -1,
baseVisualShapeIndex=clone_visual_shape(body, base_link, client) if visual else -1,
basePosition=base_point,
baseOrientation=base_quat,
baseInertialFramePosition=base_dynamics_info.local_inertial_pos,
baseInertialFrameOrientation=base_dynamics_info.local_inertial_orn,
linkMasses=masses,
linkCollisionShapeIndices=collision_shapes,
linkVisualShapeIndices=visual_shapes,
linkPositions=positions,
linkOrientations=orientations,
linkInertialFramePositions=inertial_positions,
linkInertialFrameOrientations=inertial_orientations,
linkParentIndices=parent_indices,
linkJointTypes=joint_types,
linkJointAxis=joint_axes,
physicsClientId=client)
#set_configuration(new_body, get_joint_positions(body, movable_joints)) # Need to use correct client
for joint, value in zip(range(len(links)), get_joint_positions(body, links)):
# TODO: check if movable?
p.resetJointState(new_body, joint, value, targetVelocity=0, physicsClientId=client)
return new_body
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)
