def warehouse_scene_pairwise_test(initial_state_id, goal_constraint_starts_with, robot, arm="right", planner_id=""): from warehouse_utils import MoveitWarehouseDatabase constraints_db = MoveitWarehouseDatabase('moveit_constraints') states_db = MoveitWarehouseDatabase('moveit_robot_states') initial_state = states_db.get_message(RobotState, state_id=initial_state_id) query = {'constraints_id':{"$regex":"^%s.*" % goal_constraint_starts_with}} constraints = constraints_db.get_messages(Constraints, query) print "Got", len(constraints), "goal constraints from the warehouse" initial_states = [initial_state] positions, quats = [], [] results = [] base_pose = initial_state.multi_dof_joint_state.poses[0] base_q = base_pose.orientation base_p = base_pose.position t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z]) robot.SetTransform(t) update_rave_from_ros(robot, initial_state.joint_state.position, initial_state.joint_state.name) for c in constraints: pos, quat = pose_constraints_to_xyzquat(c) post = (pos.x, pos.y, pos.z) quatt = (quat.x, quat.y, quat.z, quat.w) positions.append(post) quats.append(quatt) state = robot_state_from_pose_goal(post, quatt, arm, robot, initial_state) if state: initial_states.append(state) for start_state in initial_states: update_rave_from_ros(robot, start_state.joint_state.position, start_state.joint_state.name) for posit, quater in zip(positions, quats): result = test_plan_to_pose(posit, quater, arm, robot, start_state, planner_id, "%s_wrist_roll_link") if result is not None: results.append(result) process_results(results)
def warehouse_test(initial_state_id, goal_constraint_starts_with): from warehouse_utils import MoveitWarehouseDatabase constraints_db = MoveitWarehouseDatabase('moveit_constraints') states_db = MoveitWarehouseDatabase('moveit_robot_states') initial_state = states_db.get_message(RobotState, state_id=initial_state_id) query = {'constraints_id':{"$regex":"^%s.*" % goal_constraint_starts_with}} constraints = constraints_db.get_messages(Constraints, query) print initial_state print len(constraints) for c in constraints: print pose_constraints_to_xyzquat(c)
def warehouse_scene_pairwise_test(initial_state_id, goal_constraint_starts_with, robot, arm="right", planner_id=""): from warehouse_utils import MoveitWarehouseDatabase constraints_db = MoveitWarehouseDatabase('moveit_constraints') states_db = MoveitWarehouseDatabase('moveit_robot_states') initial_state = states_db.get_message(RobotState, state_id=initial_state_id) query = { 'constraints_id': { "$regex": "^%s.*" % goal_constraint_starts_with } } constraints = constraints_db.get_messages(Constraints, query) print "Got", len(constraints), "goal constraints from the warehouse" initial_states = [initial_state] positions, quats = [], [] results = [] base_pose = initial_state.multi_dof_joint_state.poses[0] base_q = base_pose.orientation base_p = base_pose.position t = rave.matrixFromPose( [base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z]) robot.SetTransform(t) update_rave_from_ros(robot, initial_state.joint_state.position, initial_state.joint_state.name) for c in constraints: pos, quat = pose_constraints_to_xyzquat(c) post = (pos.x, pos.y, pos.z) quatt = (quat.x, quat.y, quat.z, quat.w) positions.append(post) quats.append(quatt) state = robot_state_from_pose_goal(post, quatt, arm, robot, initial_state) if state: initial_states.append(state) for start_state in initial_states: update_rave_from_ros(robot, start_state.joint_state.position, start_state.joint_state.name) for posit, quater in zip(positions, quats): result = test_plan_to_pose(posit, quater, arm, robot, start_state, planner_id, "%s_wrist_roll_link") if result is not None: results.append(result) process_results(results)
def warehouse_test(initial_state_id, goal_constraint_starts_with): from warehouse_utils import MoveitWarehouseDatabase constraints_db = MoveitWarehouseDatabase('moveit_constraints') states_db = MoveitWarehouseDatabase('moveit_robot_states') initial_state = states_db.get_message(RobotState, state_id=initial_state_id) query = { 'constraints_id': { "$regex": "^%s.*" % goal_constraint_starts_with } } constraints = constraints_db.get_messages(Constraints, query) print initial_state print len(constraints) for c in constraints: print pose_constraints_to_xyzquat(c)