def check_valid_kinematics(robot, way_points, phi, theta, collision_fn):
ee_yaw = sample_ee_yaw()
print_orientation = make_print_pose(phi, theta, ee_yaw)
for way_pt in way_points:
world_from_ee_tip = multiply(Pose(point=Point(*way_pt)),
print_orientation)
conf = sample_tool_ik(robot, world_from_ee_tip)
if not conf or collision_fn(conf):
# conf not exist or collision
return False
return True
def gen_fn(index, pose, grasp):
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles = list(obstacle_from_name.values()) # + [body]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles=obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits=get_custom_limits(robot))
attach_pose = multiply(pose.value, invert(grasp.attach))
approach_pose = multiply(attach_pose, (approach_vector, unit_quat()))
# approach_pose = multiply(pose.value, invert(grasp.approach))
for _ in range(max_attempts):
if USE_IKFAST:
attach_conf = sample_tool_ik(robot, attach_pose)
else:
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if (attach_conf is None) or collision_fn(attach_conf):
continue
set_joint_positions(robot, movable_joints, attach_conf)
#if USE_IKFAST:
# approach_conf = sample_tool_ik(robot, approach_pose, nearby_conf=attach_conf)
#else:
approach_conf = inverse_kinematics(robot, tool_link, approach_pose)
if (approach_conf is None) or collision_fn(approach_conf):
continue
set_joint_positions(robot, movable_joints, approach_conf)
path = plan_direct_joint_motion(
robot,
movable_joints,
attach_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions)
if path is None: # TODO: retreat
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
def solve_ik(end_effector, target_pose, nearby=True):
robot = end_effector.robot
movable_joints = get_movable_joints(robot)
if USE_IKFAST:
# TODO: sample from the set of solutions
conf = sample_tool_ik(robot,
target_pose,
closest_only=nearby,
get_all=False)
else:
# TODO: repeat several times
# TODO: condition on current config
if not nearby:
# randomly sample and set joint conf for the pybullet ik fn
sample_fn = get_sample_fn(robot, movable_joints)
set_joint_positions(robot, movable_joints, sample_fn())
# note that the conf get assigned inside this ik fn right away!
conf = inverse_kinematics(robot, end_effector.tool_link, target_pose)
return conf
def check_cap_vert_feasibility(robot, poses, cap_rung, cap_vert):
assert(isinstance(cap_rung, CapRung) and isinstance(cap_vert, CapVert))
assert(cap_rung.path_pts)
assert(len(cap_rung.path_pts) == len(poses))
# check ik feasibility for each of the path points
for i, pose in enumerate(poses):
jt_list = sample_tool_ik(robot, pose, get_all=True)
jt_list = [jts for jts in jt_list if jts and not cap_rung.collision_fn(jts)]
if not jt_list or all(not jts for jts in jt_list):
# print('#{0}/{1} path pt break'.format(i,len(poses)))
return None
else:
# only store the first and last sol
if i == 0:
cap_vert.st_jt_data = [jt for jt_l in jt_list for jt in jt_l]
if i == len(poses)-1:
cap_vert.end_jt_data = [jt for jt_l in jt_list for jt in jt_l]
return cap_vert
def generate_ladder_graph_from_poses(robot, dof, pose_list, collision_fn=lambda x: False, dt=-1):
# TODO: lazy collision check
graph = LadderGraph(dof)
graph.resize(len(pose_list))
# solve ik for each pose, build all rungs (w/o edges)
for i, pose in enumerate(pose_list):
jt_list = sample_tool_ik(robot, pose, get_all=True)
jt_list = [jts for jts in jt_list if jts and not collision_fn(jts)]
if not jt_list or all(not jts for jts in jt_list):
return None
graph.assign_rung(i, jt_list)
# build edges
for i in range(graph.get_rungs_size()-1):
st_id = i
end_id = i + 1
jt1_list = graph.get_data(st_id)
jt2_list = graph.get_data(end_id)
st_size = graph.get_rung_vert_size(st_id)
end_size = graph.get_rung_vert_size(end_id)
edge_builder = EdgeBuilder(st_size, end_size, dof)
for k in range(st_size):
st_id = k * dof
for j in range(end_size):
end_id = j * dof
edge_builder.consider(jt1_list[st_id : st_id+dof], jt2_list[end_id : end_id+dof], j)
edge_builder.next(k)
edges = edge_builder.result
if not edge_builder.has_edges and DEBUG:
print('no edges!')
graph.assign_edges(i, edges)
return graph