'''
ee = fr.ee(q)
err = np.sum((np.asarray(desired_ee_rp) - np.asarray(ee[3:5]))**2)
grad = np.asarray([
0, 0, 0, 2 * (ee[3] - desired_ee_rp[0]),
2 * (ee[4] - desired_ee_rp[1]), 0
])
return err, grad
print('Desired ee roll and pitch:', desired_ee_rp)
joints_start = fr.home_joints.copy()
joints_start[0] = -np.deg2rad(45)
joints_target = joints_start.copy()
joints_target[0] = np.deg2rad(45)
rrt = RRT(fr, is_in_collision)
constraint = ee_upright_constraint
plan = rrt.plan(joints_start, joints_target, constraint)
i = 0
collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes')
rate = rospy.Rate(10)
while not rospy.is_shutdown():
joints = plan[i % len(plan)]
fr.publish_joints(joints)
fr.publish_collision_boxes(joints)
collision_boxes_publisher.publish_boxes(boxes)
i += 1
rate.sleep()
if args.rrt:
print("RRT: RRT planner is selected!")
planner = RRT(fr, is_in_collision)
elif args.rrtc:
print("RRTC: RRT Connect planner is selected!")
planner = RRTConnect(fr, is_in_collision)
elif args.prm:
print("PRM: PRM planner is selected!")
planner = PRM(fr, is_in_collision)
elif args.obprm:
print("OB_PRM: OB_PRM planner is selected!")
planner = OBPRM(fr, is_in_collision)
constraint = ee_upright_constraint
if args.prm or args.obprm:
plan = planner.plan(joints_start, joints_target, constraint, args)
else:
plan = planner.plan(joints_start, joints_target, constraint)
path_quality = get_plan_quality(plan)
print("Path quality: {}".format(path_quality))
collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes')
rate = rospy.Rate(10)
i = 0
while not rospy.is_shutdown():
rate.sleep()
joints = plan[i % len(plan)]
fr.publish_joints(joints)
fr.publish_collision_boxes(joints)
collision_boxes_publisher.publish_boxes(boxes)
inputFile = 'maps/prison.txt'
# ruins
elif case == 8:
inputFile = 'maps/ruins.txt'
# school
elif case == 9:
inputFile = 'maps/school.txt'
# shooting_range
elif case == 10:
inputFile = 'maps/shooting_range.txt'
else:
exit('Error: no such map')
# process input maps
file = pf(inputFile)
s, g, obs, polyObs = file.pc()
# run rrt
exploreArea = [0, 15]
rrt = RRT(s, g, obs, polyObs, exploreArea, improved=False, stepRadius=0.9)
rrt.plan() # grow rtt
rrt.pathStat() # print path finding statistic
raw_input('====> press enter to terminate the program')
if __name__ == '__main__':
main()