def ompl_plan(robot, group_name, active_joint_names, active_affine,
target_dof_values, init_trajs):
import moveit_msgs.msg as mm
from planning_benchmark_common.rave_env_to_ros import rave_env_to_ros, ros_joints_to_rave
import rospy
ps = rave_env_to_ros(robot.GetEnv())
msg = mm.MotionPlanRequest()
msg.group_name = group_name
msg.planner_id = args.ompl_planner_id
msg.allowed_planning_time = args.max_planning_time
c = mm.Constraints()
joints = robot.GetJoints()
if active_affine == 0:
base_joint_names = []
elif active_affine == 11:
base_joint_names = [
"world_joint/x", "world_joint/y", "world_joint/theta"
]
else:
raise Exception
for (name, val) in zip(active_joint_names + base_joint_names,
target_dof_values):
c.joint_constraints.append(
mm.JointConstraint(joint_name=name,
position=val,
weight=1,
tolerance_above=.0001,
tolerance_below=.0001))
msg.start_state = ps.robot_state
msg.goal_constraints = [c]
svc = get_ompl_service()
try:
t_start = time()
svc_response = svc.call(msg)
response = svc_response.motion_plan_response
# success
joint_names = response.trajectory.joint_trajectory.joint_names
pts = response.trajectory.joint_trajectory.points
base_pts = response.trajectory.multi_dof_joint_trajectory.points
traj = []
for i, p in enumerate(pts):
row = ros_joints_to_rave(robot, joint_names, p.positions)[0]
if base_pts:
base_trans = base_pts[i].transforms[0]
row += [base_trans.translation.x, base_trans.translation.y]
row += [ros_quat_to_aa(base_trans.rotation)[2]]
traj.append(row)
traj = np.asarray(traj)
return PlannerStatus.SUCCESS, response.planning_time, traj, ''
except rospy.service.ServiceException, e:
return PlannerStatus.FAIL_TIMEOUT, time(
) - t_start, [], 'failed due to ServiceException: ' + repr(e)
def ompl_plan(robot, group_name, active_joint_names, active_affine, target_dof_values, init_trajs):
import moveit_msgs.msg as mm
from planning_benchmark_common.rave_env_to_ros import rave_env_to_ros, ros_joints_to_rave
import rospy
ps = rave_env_to_ros(robot.GetEnv())
msg = mm.MotionPlanRequest()
msg.group_name = group_name
msg.planner_id = args.ompl_planner_id
msg.allowed_planning_time = args.max_planning_time
c = mm.Constraints()
joints = robot.GetJoints()
if active_affine == 0:
base_joint_names = []
elif active_affine == 11:
base_joint_names = ["world_joint/x", "world_joint/y", "world_joint/theta"]
else:
raise Exception
for (name, val) in zip(active_joint_names+base_joint_names, target_dof_values):
c.joint_constraints.append(mm.JointConstraint(joint_name=name, position = val,weight=1,tolerance_above=.0001, tolerance_below=.0001))
msg.start_state = ps.robot_state
msg.goal_constraints = [c]
svc = get_ompl_service()
try:
t_start = time()
svc_response = svc.call(msg)
response = svc_response.motion_plan_response
# success
joint_names = response.trajectory.joint_trajectory.joint_names
pts = response.trajectory.joint_trajectory.points
base_pts = response.trajectory.multi_dof_joint_trajectory.points
traj = []
for i, p in enumerate(pts):
row = ros_joints_to_rave(robot, joint_names, p.positions)[0]
if base_pts:
base_trans = base_pts[i].transforms[0]
row += [base_trans.translation.x, base_trans.translation.y]
row += [ros_quat_to_aa(base_trans.rotation)[2]]
traj.append(row)
traj = np.asarray(traj)
return True, response.planning_time, traj, ''
except rospy.service.ServiceException, e:
return False, time() - t_start, [], 'failed due to ServiceException: ' + repr(e)
def setup_ompl(env):
import rospy
from planning_benchmark_common.rave_env_to_ros import rave_env_to_ros
rospy.init_node("benchmark_ompl",disable_signals=True)
get_ompl_service()
rave_env_to_ros(env)
def setup_ompl(env):
import rospy
from planning_benchmark_common.rave_env_to_ros import rave_env_to_ros
rospy.init_node("benchmark_ompl", disable_signals=True)
get_ompl_service()
rave_env_to_ros(env)
