def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
for pose in interpolate_poses(start_pose, end_pose, pos_step_size=0.01):
conf = next(
closest_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
wait_for_user()
def test_ik(robot, info, tool_link, tool_pose):
draw_pose(tool_pose)
# TODO: sort by one joint angle
# TODO: prune based on proximity
ik_joints = get_ik_joints(robot, info, tool_link)
for conf in either_inverse_kinematics(robot,
info,
tool_link,
tool_pose,
use_pybullet=False,
max_distance=INF,
max_time=10,
max_candidates=INF):
# TODO: profile
set_joint_positions(robot, ik_joints, conf)
wait_for_user()
def main(num_iterations=10):
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
side = 0.05
block = create_box(w=side, l=side, h=side, color=RED)
start_time = time.time()
with LockRenderer():
with HideOutput():
# TODO: MOVO must be loaded last
robot = load_model(MOVO_URDF, fixed_base=True)
#set_all_color(robot, color=MOVO_COLOR)
assign_link_colors(robot)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
print('Load time: {:.3f}'.format(elapsed_time(start_time)))
dump_body(robot)
#print(get_colliding(robot))
#for arm in ARMS:
# test_close_gripper(robot, arm)
#test_grasps(robot, block)
arm = RIGHT
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_info = MOVO_INFOS[arm]
print_ik_warning(ik_info)
ik_joints = get_ik_joints(robot, ik_info, tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
wait_if_gui('Start?')
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(num_iterations):
conf = sample_fn()
print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations,
np.array(conf)))
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_if_gui()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_if_gui()
test_retraction(robot,
ik_info,
tool_link,
fixed_joints=fixed_joints,
max_time=0.05,
max_candidates=100)
disconnect()
