def goalPose_feasibility_check_act(self, data):
print "srv start"
f_check_act = rospy.ServiceProxy('feasibile_check_act', GetMotionPlan)
pub_msg = GetMotionPlanRequest()
pub_msg.workspace_parameters = self.scene
pub_msg.planner_id ='BiTRRT'
pub_msg.group_name = data.arm_name[0]
pub_msg.num_planning_attempts =50000
pub_msg.allowed_planning_time =5
pub_msg.goal_constraints.position_constraints = data.goal_position
pub_msg.goal_constraints.orientation_constraints = data.goal_orientation
# plan = move_group.plan()
print pub_msg
f_check_act.send_goal(pub_msg)
ret = f_check_act.get_result()
print ret
return arm_move_srvResponse(
w_flag=1,
feasibility=0,
r_trj=1
)
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name, plan_type='pfs'):
"""
Given a start and goal point, plan by classical planner.
Args:
start_point (list of float): A starting joint configuration.
goal_point (list of float): A goal joint configuration.
planner_number (int): The index of the planner to be used as
returned by acquire_planner().
joint_names (list of str): The name of the joints corresponding to
start_point and goal_point.
group_name (str): The name of the group to which the joint names
correspond.
planning_time (float): Maximum allowed time for planning, in seconds.
planner_config_name (str): Type of planner to use.
plan_type (str): either pfs or rr. If rr we don't use the path length threshold.
Return:
list of list of float: A sequence of points representing the joint
configurations at each point on the path.
"""
planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan)
rospy.loginfo("%s Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" \
% (rospy.get_name(), self.planners[planner_number], start_point, goal_point))
# Put together the service request.
req = GetMotionPlanRequest()
req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime()
req.motion_plan_request.group_name = group_name
req.motion_plan_request.num_planning_attempts = 1
req.motion_plan_request.allowed_planning_time = planning_time
req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default
req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_point
req.motion_plan_request.goal_constraints.append(Constraints())
req.motion_plan_request.goal_constraints[0].joint_constraints = []
for i in xrange(len(joint_names)):
temp_constraint = JointConstraint()
temp_constraint.joint_name = joint_names[i]
temp_constraint.position = goal_point[i]
temp_constraint.tolerance_above = 0.05;
temp_constraint.tolerance_below = 0.05;
req.motion_plan_request.goal_constraints[0].joint_constraints.append(temp_constraint)
#call the planner
rospy.wait_for_service(self.planners[planner_number])
rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name)
plan_time = np.inf
try:
plan_time = time.time()
response = planner_client(req)
plan_time = time.time() - plan_time
except rospy.ServiceException, e:
rospy.loginfo("%s Plan Trajectory Wrapper: service call failed: %s"
% (rospy.get_name(), e))
return plan_time, None
def _create_get_motion_plan_request(self, start_point, goal_point, group_name, joint_names, planning_time):
# Just puts together a MotionPlanRequest to send to the lightning node.
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = group_name
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_point
req.motion_plan_request.goal_constraints.append(Constraints())
req.motion_plan_request.goal_constraints[0].joint_constraints = []
for i in xrange(len(joint_names)):
tempConstraint = JointConstraint()
tempConstraint.joint_name = joint_names[i]
tempConstraint.position = goal_point[i]
req.motion_plan_request.goal_constraints[0].joint_constraints.append(tempConstraint)
req.motion_plan_request.allowed_planning_time = planning_time
return req
def quick_motion_test_client():
rospy.wait_for_service('/lightning/lightning_get_path')
print "Working on it..."
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = "torso"
req.motion_plan_request.start_state.joint_state.name = ["torso_lift_joint"]
req.motion_plan_request.start_state.joint_state.position = [float(0.2)]
tempConstraint = JointConstraint()
tempConstraint.joint_name = req.motion_plan_request.start_state.joint_state.name[
0]
tempConstraint.position = float(0.15)
fullConstraint = Constraints()
fullConstraint.joint_constraints = [tempConstraint]
req.motion_plan_request.goal_constraints = [fullConstraint]
req.motion_plan_request.allowed_planning_time = 50.0
try:
client_func = rospy.ServiceProxy('/lightning/lightning_get_path',
GetMotionPlan)
res = client_func(req)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def plan_between_states(group_name,
joint_state_A,
joint_state_B,
robot_state=None):
"""
Plan trajectory between two joint states.
"""
# Create robot state
robot_state = joint_to_robot_state(joint_state_A, robot_state)
# Prepare motion plan request
req = GetMotionPlanRequest()
req.motion_plan_request.start_state = robot_state
req.motion_plan_request.group_name = group_name
req.motion_plan_request.planner_id = "RRTConnectkConfigDefault"
constraints = Constraints()
constraints.name = "goal"
for name, pos in zip(joint_state_B.name, joint_state_B.position):
joint_constraints = JointConstraint()
joint_constraints.joint_name = name
joint_constraints.position = pos
joint_constraints.tolerance_above = 0.001
joint_constraints.tolerance_below = 0.001
joint_constraints.weight = 100
constraints.joint_constraints.append(joint_constraints)
req.motion_plan_request.goal_constraints.append(constraints)
# Call planner
planner_srv = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
try:
res = planner_srv(req)
except rospy.service.ServiceException, e:
rospy.logerr("Service 'plan_kinematic_path' call failed: %s", str(e))
return None
def plan(args):
global responded, exception
rospy.init_node('lightning_client')
print('loading...')
if args.env_type == 's2d':
IsInCollision = plan_s2d.IsInCollision
data_loader = data_loader_2d
# create an SE2 state space
time_limit = 15.
ratio = 1.
elif args.env_type == 'c2d':
IsInCollision = plan_c2d.IsInCollision
data_loader = data_loader_2d
# create an SE2 state space
time_limit = 60.
ratio = 1.
elif args.env_type == 'r2d':
IsInCollision = plan_r2d.IsInCollision
data_loader = data_loader_r2d
# create an SE2 state space
time_limit = 60.
ratio = 1.05
elif args.env_type == 'r3d':
IsInCollision = plan_r3d.IsInCollision
data_loader = data_loader_r3d
# create an SE2 state space
time_limit = 15.
ratio = 1.
test_data = data_loader.load_test_dataset(N=args.N,
NP=args.NP,
s=args.env_idx,
sp=args.path_idx,
folder=args.data_path)
obcs, obs, paths, path_lengths = test_data
obcs = obcs.tolist()
obs = obs.tolist()
#paths = paths
path_lengths = path_lengths.tolist()
time_env = []
time_total = []
fes_env = [] # list of list
valid_env = []
# setup publisher
obc_pub = rospy.Publisher('lightning/obstacles/obc',
Float64Array2D,
queue_size=10)
obs_pub = rospy.Publisher('lightning/obstacles/obs',
Float64Array,
queue_size=10)
obs_i_pub = rospy.Publisher('lightning/obstacles/obs_i',
Int32,
queue_size=10)
length_pub = rospy.Publisher('lightning/planning/path_length_threshold',
Float64,
queue_size=10)
for i in xrange(len(paths)):
time_path = []
fes_path = [] # 1 for feasible, 0 for not feasible
valid_path = [] # if the feasibility is valid or not
# save paths to different files, indicated by i
# feasible paths for each env
obc = obcs[i]
# publishing to ROS topic
obc_msg = Float64Array2D([Float64Array(obci) for obci in obc])
obs_msg = Float64Array(obs[i])
obs_i_msg = Int32(i)
for j in xrange(len(paths[0])):
# check if the start and goal are in collision
# if so, then continue
fp = 0 # indicator for feasibility
print("step: i=" + str(i) + " j=" + str(j))
if path_lengths[i][j] == 0:
# invalid, feasible = 0, and path count = 0
fp = 0
valid_path.append(0)
elif IsInCollision(paths[i][j][0], obc) or IsInCollision(
paths[i][j][path_lengths[i][j] - 1], obc):
# invalid, feasible = 0, and path count = 0
fp = 0
valid_path.append(0)
else:
valid_path.append(1)
# obtaining the length threshold for planning
data_length = 0.
for k in xrange(path_lengths[i][j] - 1):
data_length += np.linalg.norm(paths[i][j][k + 1] -
paths[i][j][k])
length_msg = Float64(data_length * ratio)
# call lightning service
request = GetMotionPlanRequest()
request.motion_plan_request.group_name = 'base'
for k in xrange(len(paths[i][j][0])):
request.motion_plan_request.start_state.joint_state.name.append(
'%d' % (k))
request.motion_plan_request.start_state.joint_state.position.append(
paths[i][j][0][k])
request.motion_plan_request.goal_constraints.append(
Constraints())
for k in xrange(len(paths[i][j][0])):
request.motion_plan_request.goal_constraints[
0].joint_constraints.append(JointConstraint())
request.motion_plan_request.goal_constraints[
0].joint_constraints[k].position = paths[i][j][
path_lengths[i][j] - 1][k]
request.motion_plan_request.allowed_planning_time = time_limit
responded = False
exception = False
def publisher():
global responded, exception
while not responded and not exception:
print('sending obstacle message...')
obc_pub.publish(obc_msg)
obs_pub.publish(obs_msg)
obs_i_pub.publish(obs_i_msg)
length_pub.publish(length_msg)
rospy.sleep(0.5)
pub_thread = threading.Thread(target=publisher, args=())
pub_thread.start()
print('waiting for lightning service...')
try:
# to make sure when we CTRL+C we can exit
rospy.wait_for_service(LIGHTNING_SERVICE)
except:
exception = True
pub_thread.join()
# exit because there is error
print(
'exception occurred when waiting for lightning service...'
)
raise
print('acquired lightning service')
lightning = rospy.ServiceProxy(LIGHTNING_SERVICE,
GetMotionPlan)
try:
respond = lightning(request)
except:
exception = True
responded = True
pub_thread.join()
if respond.motion_plan_response.error_code.val == respond.motion_plan_response.error_code.SUCCESS:
# succeed
time = respond.motion_plan_response.planning_time
time_path.append(time)
path = respond.motion_plan_response.trajectory.joint_trajectory.points
path = [p.positions for p in path]
path = np.array(path)
print(path)
# feasibility check this path
if plan_general.feasibility_check(path,
obc,
IsInCollision,
step_sz=0.01):
fp = 1
print('feasible')
else:
fp = 0
print('not feasible')
fes_path.append(fp)
time_env.append(time_path)
time_total += time_path
print('average test time up to now: %f' % (np.mean(time_total)))
fes_env.append(fes_path)
valid_env.append(valid_path)
print(np.sum(np.array(fes_env)))
print('accuracy up to now: %f' %
(float(np.sum(np.array(fes_env))) / np.sum(np.array(valid_env))))
pickle.dump(time_env, open(args.res_path + 'time.p', "wb"))
f = open(os.path.join(args.res_path, 'accuracy.txt'), 'w')
valid_env = np.array(valid_env).flatten()
fes_env = np.array(
fes_env).flatten() # notice different environments are involved
suc_rate = float(fes_env.sum()) / valid_env.sum()
f.write(str(suc_rate))
f.close()