def get_link_pose(body, link):
from pybullet_planning.interfaces.env_manager.pose_transformation import get_pose
if link == BASE_LINK:
return get_pose(body)
# if set to 1 (or True), the Cartesian world position/orientation will be recomputed using forward kinematics.
link_state = get_link_state(body,
link) #, kinematics=True, velocity=False)
return link_state.worldLinkFramePosition, link_state.worldLinkFrameOrientation
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 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_base_values(body):
return base_values_from_pose(get_pose(body))
def get_quat(body):
return get_pose(body)[1] # [x,y,z,w]
def get_point(body):
return get_pose(body)[0]
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 plan_wholebody_motion(cube_body,
joints,
finger_body,
finger_joints,
end_conf,
current_tip_pos,
cube_tip_pos,
ik,
obstacles=[],
attachments=[],
init_joint_conf=None,
disabled_collisions=set(),
weights=None,
resolutions=None,
max_distance=MAX_DISTANCE,
custom_limits={},
diagnosis=False,
vis_fn=None,
use_rrt=False,
use_incremental_rrt=False,
use_ori=False,
goal_threshold=0.1,
additional_collision_fn=None,
**kwargs):
from pybullet_planning.interfaces.env_manager.pose_transformation import get_pose
from pybullet_planning.motion_planners.utils import weighted_pose_error, weighted_position_error, weighted_paired_pose_error
assert len(joints) == len(end_conf)
assert additional_collision_fn is None or callable(additional_collision_fn)
def collision_fn(*args, **kwargs):
org_collision_fn = get_cube_tip_collision_fn(
cube_body,
finger_body,
finger_joints,
obstacles=obstacles,
attachments=attachments,
disabled_collisions=disabled_collisions,
diagnosis=diagnosis,
vis_fn=vis_fn,
max_distance=max_distance)
if additional_collision_fn is None:
return org_collision_fn(*args, **kwargs)
return org_collision_fn(*args, **kwargs) or additional_collision_fn(
*args, **kwargs)
sample_fn = get_sample_fn(cube_body, joints, custom_limits=custom_limits)
sample_joint_conf_fn = get_sample_fn(finger_body,
finger_joints,
custom_limits={})
if use_ori:
weight_fn = weighted_paired_pose_error
distance_fn = get_paired_distance_fn(cube_body,
joints,
weight_fn=weight_fn)
else:
weight_fn = weighted_position_error
distance_fn = get_distance_fn(cube_body,
joints,
weights=None,
weight_fn=weight_fn)
extend_fn = get_extend_fn(cube_body, joints, resolutions=resolutions)
# obtain start configuration
start_pos, start_quat = get_pose(cube_body)
start_ori = p.getEulerFromQuaternion(start_quat)
start_conf = np.concatenate([start_pos, start_ori]).reshape(-1)
calc_tippos_fn = get_calc_tippos_fn(current_tip_pos, cube_tip_pos,
start_pos, start_quat)
if use_rrt:
def goal_test(q):
d = distance_fn(end_conf, q)
return d <= goal_threshold
from pybullet_planning.motion_planners.wholebody_rrt_connect import rrt_goal_sample
def sample_goal():
sample = rrt_goal_sample(goal_test, end_conf, goal_threshold,
use_ori)
return sample
return wholebody_rrt(start_conf, end_conf, sample_goal, goal_test,
distance_fn, sample_fn, extend_fn, collision_fn,
calc_tippos_fn, sample_joint_conf_fn, ik,
**kwargs)
elif use_incremental_rrt:
from pybullet_planning.motion_planners.wholebody_rrt_connect import rrt_goal_sample
return wholebody_incremental_rrt(start_conf, end_conf, use_ori,
distance_fn, sample_fn, extend_fn,
collision_fn, calc_tippos_fn,
sample_joint_conf_fn, ik, **kwargs)
else:
return wholebody_birrt(start_conf,
end_conf,
distance_fn,
sample_fn,
extend_fn,
collision_fn,
calc_tippos_fn,
sample_joint_conf_fn,
ik,
init_joint_conf=init_joint_conf,
**kwargs)
