def workspace_traj(oracle, vector, manip_name=None): # TODO - use IK to check if even possibly valid
distance, direction = length(vector), normalize(vector)
steps = 10 # NOTE - relates into the number of analytical IK calls. 10 seems to be a magic number.
step_length = distance / steps
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):
try: # TODO - Bug in specifying minsteps. Need to specify at least 2 times the desired otherwise it stops early
return TrajTrajectory(
CSpace.robot_arm(oracle.base_manip.robot.GetActiveManipulator()),
oracle.base_manip.MoveHandStraight(
direction,
minsteps=10 * steps,
maxsteps=steps,
steplength=step_length,
ignorefirstcollision=None,
starteematrix=None,
greedysearch=True,
execute=False,
outputtraj=None,
maxdeviationangle=None,
planner=None,
outputtrajobj=True,
),
)
except planning_error:
return None
def inverse_kinematics(oracle, manip_trans): # 0.005 sec
with oracle.robot:
oracle.robot.SetActiveDOFs(get_arm_indices(oracle)) # TODO - is this slow and/or necessary?
with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
config = oracle.robot.GetActiveManipulator().FindIKSolution(manip_trans, 0)
if config is None: return None
set_config(oracle.robot, config, oracle.robot.GetActiveManipulator().GetArmIndices())
if robot_collision(oracle): return None
return config
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 linear_arm_traj(oracle, end_config, 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):
start_config = get_active_config(oracle.robot)
extend = extend_fn(oracle.robot)
collision = collision_fn(oracle.env, oracle.robot)
path = [start_config] + list(extend(start_config, end_config))
if any(collision(q) for q in path):
return None
return PathTrajectory(CSpace.robot_arm(oracle.robot.GetActiveManipulator()), path)
