<commit_before>import herbpy, openravepy, numpy

def look_at_hand(robot, manipulator):
    target = manipulator.GetEndEffectorTransform()[0:3, 3]
    robot.LookAt(target, execute=True)

simulation = False
left_home_config = numpy.array([ 2.60, -1.90,  0.00,  2.20,  0.00,  0.00,  0.00 ])
left_relaxed_config = numpy.array([ 0.55, -1.82,  0.35,  1.87, -2.22, -0.66, -0.98 ])
right_home_config = numpy.array([ 3.68, -1.90, 0.00, 2.20, 0.00, 0.00, 0.00 ])
right_relaxed_config = numpy.array([ 5.73, -1.82, -0.35, 1.87, -4.06, -0.66, 0.98 ])

openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging();

if simulation:
    env, robot = herbpy.initialize_sim(attach_viewer=False)
else:
    env, robot = herbpy.initialize_real(left_ft_sim=True, right_ft_sim=True)
    #env, robot = herbpy.initialize_real()
    #env, robot = herbpy.initialize_herb(attach_viewer=True)

with env:
    robot.SetActiveManipulator(robot.right_arm)
    robot.SetActiveDOFs(robot.right_arm.GetArmIndices())

if simulation:
    robot.SetActiveDOFValues(right_relaxed_config)
    robot.SetTransform([[ 1, 0, 0, -1.25 ],
                        [ 0, 1, 0,  0.49 ],
                        [ 0, 0, 1,  0.00 ],
                        [ 0, 0, 0,     1 ]])

robot.chomp_planner.ComputeDistanceField()

#traj = robot.cbirrt_planner.PlanToConfiguration(right_home_config)

# TODO: Implement synchronized trajectories.
# TODO: Use different or_owd_controller flags for each controller instance.
# TODO: Wait for the hand to finish closing.
# FIXME: WaitForController doesn't work.
# FIXME: CHOMP always returns success.
if False:
    goal_pose = robot.right_arm.GetEndEffectorTransform()
    goal_pose[0:3, 3] += numpy.array([ 0, 0, 0.4 ])

    traj = robot.cbirrt_planner.PlanToEndEffectorPose(goal_pose)
    blended_traj = robot.BlendTrajectory(traj)
    annotated_traj = robot.AddTrajectoryFlags(blended_traj, stop_on_stall=True,
        stop_on_ft=True, force_direction=[0,-1,0], force_magnitude=5, torque=[100,100,100]
    )<commit_msg>Got rid of bad FT flags in default test script/<commit_after>import herbpy, openravepy, numpy

def look_at_hand(robot, manipulator):
    target = manipulator.GetEndEffectorTransform()[0:3, 3]
    robot.LookAt(target, execute=True)

simulation = False
left_home_config = numpy.array([ 2.60, -1.90,  0.00,  2.20,  0.00,  0.00,  0.00 ])
left_relaxed_config = numpy.array([ 0.55, -1.82,  0.35,  1.87, -2.22, -0.66, -0.98 ])
right_home_config = numpy.array([ 3.68, -1.90, 0.00, 2.20, 0.00, 0.00, 0.00 ])
right_relaxed_config = numpy.array([ 5.73, -1.82, -0.35, 1.87, -4.06, -0.66, 0.98 ])

openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging();

if simulation:
    env, robot = herbpy.initialize_sim(attach_viewer=False)
else:
    #env, robot = herbpy.initialize_real(left_ft_sim=True, right_ft_sim=True)
    env, robot = herbpy.initialize_real()
    #env, robot = herbpy.initialize_herb(attach_viewer=True)

with env:
    robot.SetActiveManipulator(robot.right_arm)
    robot.SetActiveDOFs(robot.right_arm.GetArmIndices())

if simulation:
    robot.SetActiveDOFValues(right_relaxed_config)
    robot.SetTransform([[ 1, 0, 0, -1.25 ],
                        [ 0, 1, 0,  0.49 ],
                        [ 0, 0, 1,  0.00 ],
                        [ 0, 0, 0,     1 ]])

robot.chomp_planner.ComputeDistanceField()

#traj = robot.cbirrt_planner.PlanToConfiguration(right_home_config)

# TODO: Implement synchronized trajectories.
# TODO: Use different or_owd_controller flags for each controller instance.
# TODO: Wait for the hand to finish closing.
# FIXME: WaitForController doesn't work.
# FIXME: CHOMP always returns success.
if False:
    goal_pose = robot.right_arm.GetEndEffectorTransform()
    goal_pose[0:3, 3] += numpy.array([ 0, 0, 0.4 ])

    traj = robot.cbirrt_planner.PlanToEndEffectorPose(goal_pose)
    blended_traj = robot.BlendTrajectory(traj)
    annotated_traj = robot.AddTrajectoryFlags(blended_traj, stop_on_stall=True,
        stop_on_ft=True, force_direction=[0,-1,0], force_magnitude=5, torque=[100,100,100]
    )
{'fraction_non_alphanumeric': 0.13389830508474576, 'fraction_numerical': 0.06246973365617434, 'mean_word_length': 3.753739930955121, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4385422', 'n_tokens_mistral': 1728, 'n_tokens_neox': 1575, 'n_words': 303}