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)
