def initialize_environment():
# Assume rosnode is already is initialized;
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
rospy.sleep(2)
return scene
def initialize_environment():
# Initialize environment
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0)
rospy.sleep(2)
return scene, robot
def main():
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
rospy.sleep(2)
# centre table
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 1. # position of the table in the world frame
p1.pose.position.y = 0.
p1.pose.position.z = 0.
# left table (from baxter's perspective)
p1_l = PoseStamped()
p1_l.header.frame_id = robot.get_planning_frame()
p1_l.pose.position.x = 0.5 # position of the table in the world frame
p1_l.pose.position.y = 1.
p1_l.pose.position.z = 0.
p1_l.pose.orientation.x = 0.707
p1_l.pose.orientation.y = 0.707
# right table (from baxter's perspective)
p1_r = PoseStamped()
p1_r.header.frame_id = robot.get_planning_frame()
p1_r.pose.position.x = 0.5 # position of the table in the world frame
p1_r.pose.position.y = -1.
p1_r.pose.position.z = 0.
p1_r.pose.orientation.x = 0.707
p1_r.pose.orientation.y = 0.707
# open back shelf
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 1.2 # position of the table in the world frame
p2.pose.position.y = 0.0
p2.pose.position.z = 0.75
p2.pose.orientation.x = 0.5
p2.pose.orientation.y = -0.5
p2.pose.orientation.z = -0.5
p2.pose.orientation.w = 0.5
pw = PoseStamped()
pw.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p_ob1 = PoseStamped()
p_ob1.header.frame_id = robot.get_planning_frame()
p_ob1.pose.position.x = .92
p_ob1.pose.position.y = 0.3
p_ob1.pose.position.z = 0.89
# the ole duck
p_ob2 = PoseStamped()
p_ob2.header.frame_id = robot.get_planning_frame()
p_ob2.pose.position.x = 0.87
p_ob2.pose.position.y = -0.4
p_ob2.pose.position.z = 0.24
# clean environment
scene.remove_world_object("table_center")
scene.remove_world_object("table_side_left")
scene.remove_world_object("table_side_right")
scene.remove_world_object("shelf")
scene.remove_world_object("wall")
scene.remove_world_object("part")
scene.remove_world_object("duck")
rospy.sleep(1)
scene.add_box("table_center", p1,
size=(.5, 1.5, 0.4)) # dimensions of the table
scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4))
scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4))
scene.add_mesh(
"shelf",
p2,
"/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/bookshelf_light.stl",
size=(.025, .01, .01))
scene.add_plane("wall", pw, normal=(0, 1, 0))
part_size = (0.07, 0.05, 0.12)
scene.add_box("part", p_ob1, size=part_size)
scene.add_mesh(
"duck",
p_ob2,
"/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/duck.stl",
size=(.001, .001, .001))
rospy.sleep(1)
print(scene.get_known_object_names())
## ---> SET COLOURS
table_color = color_norm([105, 105,
105]) # normalize colors to range [0, 1]
shelf_color = color_norm([139, 69, 19])
duck_color = color_norm([255, 255, 0])
_colors = {}
setColor('table_center', _colors, table_color, 1)
setColor('table_side_left', _colors, table_color, 1)
setColor('table_side_right', _colors, table_color, 1)
setColor('shelf', _colors, shelf_color, 1)
setColor('duck', _colors, duck_color, 1)
sendColors(_colors, scene)
def main():
rospy.init_node(sys.argv[1])
# sometimes takes a few tries to connect to robot arm
limb_init = False
while not limb_init:
try:
limb = baxter_interface.Limb('right')
limb_init = True
except OSError:
limb_init = False
neutral_start = limb.joint_angles()
min_goal_cost_threshold = 2.0
# Set up planning scene and Move Group objects
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher(
'planning_scene', PlanningScene, queue_size=0)
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
sv = StateValidity()
set_environment(robot, scene)
# Setup tables (geometry and location defined in moveit_functions.py, set_environment function)
# set_environment(robot, scene)
# Additional Move group setup
# group.set_goal_joint_tolerance(0.001)
# group.set_max_velocity_scaling_factor(0.7)
# group.set_max_acceleration_scaling_factor(0.1)
max_time = 300
group.set_planning_time(max_time)
# Dictionary to save path data, and filename to save the data to in the end
pathsDict = {}
pathsFile = "data/path_data_example"
# load data from environment files for obstacle locations and collision free goal poses
with open("env/trainEnvironments.pkl", "rb") as env_f:
envDict = pickle.load(env_f)
with open("env/trainEnvironments_testGoals.pkl", "rb") as goal_f:
goalDict = pickle.load(goal_f)
# Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt
sceneModifier = PlanningSceneModifier(envDict['obsData'])
sceneModifier.setup_scene(scene, robot, group)
robot_state = robot.get_current_state()
rs_man = RobotState()
rs_man.joint_state.name = robot_state.joint_state.name
# Here we go
test_envs = envDict['poses'].keys() #slice this if you want only some environments
done = False
iter_num = 0
print("Testing envs: ")
print(test_envs)
while(not rospy.is_shutdown() and not done):
for i_env, env_name in enumerate(test_envs):
print("env iteration number: " + str(i_env))
print("env name: " + str(env_name))
sceneModifier.delete_obstacles()
new_pose = envDict['poses'][env_name]
sceneModifier.permute_obstacles(new_pose)
print("Loaded new pose and permuted obstacles\n")
pathsDict[env_name] = {}
pathsDict[env_name]['paths'] = []
pathsDict[env_name]['costs'] = []
pathsDict[env_name]['times'] = []
pathsDict[env_name]['total'] = 0
pathsDict[env_name]['feasible'] = 0
collision_free_goals = goalDict[env_name]['Joints']
total_paths = 0
feasible_paths = 0
i_path = 0
while (total_paths < 30): #run until either desired number of total or feasible paths has been found
#do planning and save data
# some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table
# by only taking goals which have a straight path cost above a threshold
valid_goal = False
while not valid_goal:
goal = collision_free_goals[np.random.randint(0, len(collision_free_goals))]
optimal_path = [neutral_start.values(), goal.values()]
optimal_cost = compute_cost(optimal_path)
if optimal_cost > min_goal_cost_threshold:
valid_goal = True
print("FP: " + str(feasible_paths))
print("TP: " + str(total_paths))
total_paths += 1
i_path += 1
# Uncomment below if using a start state different than the robot current state
# filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff
# filler_robot_state[10:17] = moveit_scrambler(start.values())
# rs_man.joint_state.position = tuple(filler_robot_state)
# group.set_start_state(rs_man) # set start
group.set_start_state_to_current_state()
group.clear_pose_targets()
try:
group.set_joint_value_target(moveit_scrambler(goal.values())) # set target
except MoveItCommanderException as e:
print(e)
continue
start_t = time.time()
plan = group.plan()
pos = [plan.joint_trajectory.points[i].positions for i in range(len(plan.joint_trajectory.points))]
if pos != []:
pos = np.asarray(pos)
cost = compute_cost(pos)
t = time.time() - start_t
print("Time: " + str(t))
print("Cost: " + str(cost))
# Uncomment below if using max time as criteria for failure
if (t > (max_time*0.99)):
print("Reached max time...")
continue
feasible_paths += 1
pathsDict[env_name]['paths'].append(pos)
pathsDict[env_name]['costs'].append(cost)
pathsDict[env_name]['times'].append(t)
pathsDict[env_name]['feasible'] = feasible_paths
pathsDict[env_name]['total'] = total_paths
# Uncomment below if you want to overwrite data on each new feasible path
with open (pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("\n")
sceneModifier.delete_obstacles()
iter_num += 1
print("Env: " + str(env_name))
print("Feasible Paths: " + str(feasible_paths))
print("Total Paths: " + str(total_paths))
print("\n")
pathsDict[env_name]['total'] = total_paths
pathsDict[env_name]['feasible'] = feasible_paths
with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("Done iterating, saving all data and exiting...\n\n\n")
with open(pathsFile + ".pkl", "wb") as path_f:
pickle.dump(pathsDict, path_f)
done = True
