def __init__(self):
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
self._collision_boxes_pub = CollisionBoxesPublisher('franka_collision_boxes')
self._collision_boxes_data = np.zeros((len(self.collision_box_shapes), 10))
self._collision_boxes_data[:, -3:] = self.collision_box_shapes
# Precompute things and preallocate np memory for collision checking
self._collision_box_poses = []
for pose in self._collision_box_poses_raw:
T = np.eye(4)
T[:3, 3] = pose[:3]
T[:3, :3] = quaternion.as_rotation_matrix(quaternion.quaternion(*pose[3:]))
self._collision_box_poses.append(T)
self._collision_box_hdiags = []
self._collision_box_vertices_offset = []
self._vertex_offset_signs = np.array(list(product([1, -1],[1,-1], [1,-1])))
for sizes in self.collision_box_shapes:
hsizes = sizes/2
self._collision_box_vertices_offset.append(self._vertex_offset_signs * hsizes)
self._collision_box_hdiags.append(np.linalg.norm(sizes/2))
self._collision_box_vertices_offset = np.array(self._collision_box_vertices_offset)
self._collision_box_hdiags = np.array(self._collision_box_hdiags)
self._collision_proj_axes = np.zeros((3, 15))
self._box_vertices_offset = np.ones([8, 3])
self._box_transform = np.eye(4)
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)
i += 1