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)