def vector_traj(oracle, vector, manip_name=None):
with oracle.robot:
if manip_name is not None:
oracle.robot.SetActiveManipulator(manip_name)
oracle.robot.SetActiveDOFs(get_arm_indices(oracle))
with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
end_config = inverse_kinematics(oracle, vector_trans(get_manip_trans(oracle), vector))
if end_config is None:
return None
return linear_arm_traj(oracle, end_config, manip_name=manip_name)
def resting_config_script(oracle):
set_trans(oracle.robot, unit_trans())
base_aabb = aabb_from_body(oracle.base)
robot_aabb = aabb_from_body(oracle.robot)
print base_aabb
print robot_aabb
# <-1.500975 0.000000 0.454092 0.334706 0.333375 0.328094>
hand_trans = np.dot(np.array([ # Side grasps
[1, 0, 0, 0.334706*(3./2)],
[0, 1, 0, 0.333375*(5./4)],
[0, 0, 1, 0.738471*(3./2)],
[0, 0, 0, 1]
]), get_tool_trans(oracle))
quat = quat_dot(quat_look_at(-unit_z()), quat_from_trans(get_tool_trans(oracle))) # Top grasps
point = np.array([ 0.334706*(3./2), 0.333375*(3./4), 0.738471*(2.75/2)])
hand_trans = trans_from_quat_point(quat, point)
config = inverse_kinematics(oracle, hand_trans)
print config
if config is not None:
set_config(oracle.robot, config, oracle.robot.GetActiveManipulator().GetArmIndices())
