def inverse_kinematics_helper(env, robot, manip_trans):
with robot:
robot.SetActiveDOFs(get_active_arm_indices(robot))
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
#print get_manipulator(robot).GetIkSolver()
config = get_manipulator(robot).FindIKSolution(
manip_trans, 0
) # NOTE - Finds a close solution to the current robot's joint values
if config is None: return None
set_config(robot, config, get_active_arm_indices(robot))
if env.CheckCollision(robot) or robot.CheckSelfCollision():
return None
return config
def solve_all_inverse_kinematics(env, robot, manip_trans):
with robot:
robot.SetActiveDOFs(get_active_arm_indices(robot))
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
config = get_manipulator(robot).FindIKSolution(manip_trans, 0)
#ik_param = IkParameterization(manip_trans[0:3,3],ikmodel.iktype) # build up the translation3d ik query
#ik_param = GetIkParameterization(get_manipulator(robot), ...)
configs = get_manipulator(robot).FindIKSolutions(
ik_param, openravepy_int.IkFilterOptions.CheckEnvCollisions
) # get all solutions
if config is None: return None
set_config(robot, config, get_active_arm_indices(robot))
if env.CheckCollision(robot) or robot.CheckSelfCollision():
return None
return config
def solve_inverse_kinematics_exhaustive(env, robot, manip_trans):
with robot:
#robot.SetActiveDOFs(get_active_arm_indices(robot))
#with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs): # TODO - do I want this?
with collision_saver(env, 0):
config = get_manipulator(robot).FindIKSolution(
manip_trans, openravepy_int.IkFilterOptions.CheckEnvCollisions
) # NOTE - only handles manipulator collisions
set_config(robot, config, get_active_arm_indices(robot))
if env.CheckCollision(robot) or robot.CheckSelfCollision():
return None # NOTE - not reliably avoiding collisions
return config
def feasible_pick_place(env, robot, obj, grasps, approach_config,
do_motion_planning):
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
pose = Pose(get_pose(obj))
for grasp in grasps:
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
set_config(robot, approach_config,
robot.GetActiveManipulator().GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision():
continue
grasp_config = inverse_kinematics_helper(
env, robot,
manip_trans) # NOTE - maybe need to find all IK solutions
#grasp_config = solve_inverse_kinematics_exhaustive(env, robot, manip_trans)
if grasp_config is None:
continue
set_config(robot, grasp_config, get_active_arm_indices(robot))
#approach_config = get_full_config(robot)
traj, arm_traj = None, None
if do_motion_planning:
#traj = vector_traj_helper(env, robot, approach_vector)
traj = workspace_traj_helper(base_manip, approach_vector)
if traj is None:
continue
set_config(robot, traj.end(), get_active_arm_indices(robot))
#vector_config = oracle.get_robot_config()
arm_traj = motion_plan(env,
CSpace.robot_arm(get_manipulator(robot)),
approach_config,
self_collisions=True)
if arm_traj is None:
continue
return grasp_config, traj, arm_traj
return None
def feasible_pick_place(env, robot, obj, grasps, approach_config, do_motion_planning):
t0 = time()
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
print time() - t0
#t0 = time()
#task_manip = interfaces.TaskManipulation(robot, plannername=None, maxvelmult=None, graspername=None)
#task_manip = interfaces.TaskManipulation(robot, plannername=None, maxvelmult=None, graspername=None)
#print time() - t0
#print
# Slight overhead but it appears you can just keep creating them
#0.000910043716431
#0.000546932220459
pose = Pose(get_pose(obj))
#env.Remove(obj) # NOTE - I'm just testing this now
for grasp in grasps:
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
set_config(robot, approach_config, robot.GetActiveManipulator().GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision():
print 'Collision failure'
#rospy.loginfo("Failed collision")
continue
#DrawAxes(env, manip_trans) # TODO - OpenRAVE bug
#print grasp.grasp_trans
#handles = draw_axes(env, get_trans(obj))
#handles = draw_axes(env, manip_trans)
#raw_input('Continue?')
#grasp_config = inverse_kinematics_helper(env, robot, manip_trans) # NOTE - maybe need to find all IK solutions
grasp_config = solve_inverse_kinematics_exhaustive(env, robot, manip_trans)
if grasp_config is None:
print 'IK failure'
#rospy.loginfo("Failed IK")
continue
set_config(robot, grasp_config, get_active_arm_indices(robot))
#approach_config = get_full_config(robot)
traj, arm_traj = None, None
if do_motion_planning:
#traj = vector_traj_helper(env, robot, approach_vector)
traj = workspace_traj_helper(base_manip, approach_vector)
if traj is None:
print 'Vec traj failure'
continue
set_config(robot, traj.end(), get_active_arm_indices(robot))
#vector_config = oracle.get_robot_config()
arm_traj = motion_plan(env, CSpace.robot_arm(get_manipulator(robot)), approach_config, self_collisions=True)
#arm_traj = manip_traj_helper(base_manip, approach_config, max_iterations=25, max_tries=2)
if arm_traj is None:
print 'Approach traj failure'
continue
# TODO - need to do reverse trajectory
return grasp_config, traj, arm_traj
return None