def visualize_movement(start, path):
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory,
queue_size=100) # for visualizing the robot movement;
sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = RobotTrajectory()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state, _ in path:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
# print("The joint trajectory is: %s" % trajectory)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
def convertPlanToTrajectory(path):
"""
Convert a path in OMPL to a trajectory in Moveit that could be used for display;
:param plan: A path in OMPL;
:return: a RobotTrajectory message in Moveit;
"""
trajectory = RobotTrajectory()
states = path.getStates()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state in states:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
return trajectory
def check_config(config, state_validity_checker, limb_name):
"""
Check the validity of a configuration
:param : The configurations to check against.
:param state_validity_checker: The validity checker
:param space: The space to check for
:param limb_name: The name of the limb.
:return: The validty of the configurations.
"""
# Construct a robotstate object from a dictionary
rs = RobotState()
js = JointState()
js.name = get_joint_names('right')
js.position = [config[joint] for joint in js.name]
rs.joint_state = js
result = state_validity_checker.getStateValidity(rs,
group_name=limb_name +
'_arm')
return result.valid
def test_augment_end_effector_pos():
rospy.init_node('End_effector_pos_test')
# for publishing trajectory;
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=0)
rospy.sleep(0.5)
space = initialize_space()
ss = og.SimpleSetup(space)
state_validity_checker = MoveitOMPLStateValidityChecker(
ss.getSpaceInformation())
ss.setStateValidityChecker(state_validity_checker)
start = nominal_pos(space)
file = "/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/data/sampled_data/self_and_environment_collision/right_7_test.csv"
publisher = WaypointPublisher()
label = get_collision_label_name("one_box_environment")
with open(file, mode='rb') as input_file:
csv_reader = csv.DictReader(input_file, quoting=csv.QUOTE_NONNUMERIC)
for waypoint in csv_reader:
if waypoint[label] == 1:
point = {}
for key in get_joint_names('right'):
point[key] = waypoint[key]
goal = construct_robot_state(space, point)
plan(0, start, goal, ss, space, display_trajectory_publisher)
position = Point()
position.x = waypoint['x']
position.y = waypoint['y']
position.z = waypoint['z']
# print("The position of the data point is: %s" % position)
publisher.publish_waypoints([position],
scale=[0.05, 0.05, 0.05])
time.sleep(5.0)
def generate_self_collision_dataset(start, num_joints, num_points):
"""
Generate the base self-collision dataset labels.
:return: The file that corresponds to the dataset.
"""
total_number_points = 0
# import environment
empty_environment()
# initialize validity check
space = initialize_space()
ss = og.SimpleSetup(space)
state_validity_checker = MoveitOMPLStateValidityChecker(
ss.getSpaceInformation())
# header for the dataset
limb_name = 'right'
header_written = False
header = get_joint_names(limb_name) + [SELF_COLLISION_KEY]
directory = DIRECTORY % "empty_environment"
if not os.path.exists(directory):
os.makedirs(directory)
file_name = os.path.join(
directory, get_file_name(limb_name, num_joints, num_points, start))
# statistics for tracking generation
batch_time = []
for i in range(0, int(math.ceil(float(num_points) / BATCH_SIZE))):
print("Elasped time: %s" % (time.time() - start))
print("Start generating data for batch: %s" % (i + 1))
batch_start = i * BATCH_SIZE
batch_end = batch_start + BATCH_SIZE
size = BATCH_SIZE
if batch_end >= num_points:
# We've come to the last batch
size = num_points - batch_start
batch_time_start = time.time()
samples = generate_configs(size, state_validity_checker, limb_name)
with open(file_name, mode='a') as csv_file:
# Quote header such that it is easier for reader to parse data
writer = csv.DictWriter(csv_file,
fieldnames=header,
quoting=csv.QUOTE_NONNUMERIC)
if not header_written:
writer.writeheader()
header_written = True
writer.writerows(samples)
total_number_points += size
# Save time to generate batch for later statistics
batch_time_end = time.time()
batch_time.append(batch_time_end - batch_time_start)
# output statistics;
end = time.time()
total_time = end - start
print("Finished generating self-collision dataset. " +
"Total time elapsed: %s; Total number of points: %s" %
(total_time, total_number_points))
print(
"Average time to produce a batch of size %s: %s" %
(BATCH_SIZE, reduce(lambda x, y: x + y, batch_time) / len(batch_time)))
return file_name