def start_motor(req): robot_config = settings.ROBOT_CONFIG() #set ENABLE to true tw = tc_writeRequest() tw.var_names = ["GVL.ENABLE"] tw.values = ["true"] tw.types = ["bool"] tw_res = tc.twincat_write(tw) time.sleep(0.2) #set speed tw = tc_writeRequest() tw.var_names = ["GVL.Override"] tw.types = ["real"] tw.values = ["0.0"] tw_res = tc.twincat_write(tw) time.sleep(0.2) #set RunNegative to true tw.var_names = ["GVL.RunNegative"] tw.types = ["bool"] tw.values = ["true"] tw_res = tc.twincat_write(tw) time.sleep(0.2) res = ex_start_motorResponse() res.message = "SUCCESS" return res
def change_temp(req): robot_config = settings.ROBOT_CONFIG() res = ex_change_tempResponse() res.message = "SUCCESS" tw = tc_writeRequest() tw.var_names = ["GVL.fMiniNozzleSP"] tw.types = ["lreal"] tw.values = [req.value] print colored( "[TwinCAT-Http] Setting nozzle temperature to " + str(tw.values), 'yellow') tw_res = tc.twincat_write(tw) if not req.wait: return res tr = tc_readRequest() tr.var_names = ["GVL.FieldTC1"] tr.types = ["int"] reading = tc.twincat_read(tr) while abs((float(reading.values[0]) / 10.0) - req.value) > 5.0: time.sleep(1) reading = tc.twincat_read(tr) print colored( "Waiting for temperature: " + str( (float(reading.values[0]) / 10.0)), 'yellow') return res
def reset_motor(req): robot_config = settings.ROBOT_CONFIG() tw = tc_writeRequest() tw.var_names = [ "GVL.Override", "GVL.ENABLE", "GVL.Reset", "GVL.RunPositive", "GVL.RunNegative", "GVL.Stop" ] tw.types = ["real", "bool", "bool", "bool", "bool", "bool"] tw.values = [ float(robot_config['default_override']), False, False, False, False, False ] tw_res = tc.twincat_write(tw) time.sleep(0.2) tw.var_names = ["GVL.Reset"] tw.types = ["bool"] tw.values = ["true"] tw_res = tc.twincat_write(tw) time.sleep(0.2) tw.var_names = ["GVL.Reset"] tw.types = ["bool"] tw.values = ["false"] tw_res = tc.twincat_write(tw) res = ex_reset_motorResponse() res.message = "SUCCESS" return res
def collect_state(req): robot_config = settings.ROBOT_CONFIG() srv_move_home.call(ro_move_homeRequest(speed=robot_config['sensing_speed'], wait=True)) way_poses = pp.partition_list(req.poses, partition_size=6) last_succesful_pose = settings.HOME_POSE paths = [] for pose in way_poses: planning_req = ro_plan_pathRequest(goal_pose=pose, state_pose=last_succesful_pose, session="sensing", speed=robot_config['sensing_speed']) try: res = srv_proxy_path.call(planning_req) except: continue if res.message == "SUCCESS": last_succesful_pose = pose paths.append(res.path) tag_collection = [] #move along sensing path and take snapshots between each path segment for path in paths: mv_req = ro_move_pathRequest(path=path, wait=True) srv_move_path.call(mv_req) tag_collection.append(get_tags(se_get_tagsRequest())) #go back home bc #yolo srv_move_home.call(ro_move_homeRequest(speed=robot_config['sensing_speed'], wait=True)) by_block = {} for measurement in tag_collection: i = 0 for _id in measurement.ids: if not by_block.has_key(_id): by_block[_id] = [] by_block[_id].append(measurement.tag_poses[(i*7):((i+1)*7)]) i += 1 res = se_collect_stateResponse() res.tag_poses = [] #average over all measurements by_block_average = {} for i in by_block: by_block_average[i] = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] ref_quaternion = normalize_quaternion([by_block[i][0][3], by_block[i][0][4], by_block[i][0][5], by_block[i][0][6]]) ratio = 1.0 / len(by_block[i]) for ii in range(len(by_block[i])): new_quaternion = normalize_quaternion([by_block[i][ii][3], by_block[i][ii][4], by_block[i][ii][5], by_block[i][ii][6]]) if not are_quaternions_close(ref_quaternion, new_quaternion): new_quaternion = inverse_quaternion(new_quaternion) by_block_average[i][0] += by_block[i][ii][0] * ratio by_block_average[i][1] += by_block[i][ii][1] * ratio by_block_average[i][2] += by_block[i][ii][2] * ratio by_block_average[i][3] += new_quaternion[0] * ratio by_block_average[i][4] += new_quaternion[1] * ratio by_block_average[i][5] += new_quaternion[2] * ratio by_block_average[i][6] += new_quaternion[3] * ratio #in case groups are defined if len(req.pair_lengths) > 0: #properly store group indices in list of lists groups groups = [] offsets = [0] for i in range(len(req.pair_lengths)): offsets.append(offsets[i] + req.pair_lengths[i]) groups.append(req.pairs[offsets[i]:offsets[i+1]]) for g in groups: #check if group indices are actually present in measurements g = list(g) for i in g: if not by_block_average.has_key(i): g.remove(i) if len(g) > 1: ratio = 1.0 / sum(range(len(g)+1)) rank = len(g) weighted_average = [0,0,0,0,0,0,0] ref_quaternion = normalize_quaternion([by_block_average[g[0]][3], by_block_average[g[0]][4], by_block_average[g[0]][5], by_block_average[g[0]][6]]) index = 0 for i in g: weighted_average[0] += by_block_average[i][0] * rank weighted_average[1] += by_block_average[i][1] * rank weighted_average[2] += by_block_average[i][2] * rank new_quaternion = normalize_quaternion([by_block_average[i][3], by_block_average[i][4], by_block_average[i][5], by_block_average[i][6]]) if not are_quaternions_close(ref_quaternion, new_quaternion): new_quaternion = inverse_quaternion(new_quaternion) weighted_average[3] += new_quaternion[0] * rank weighted_average[4] += new_quaternion[1] * rank weighted_average[5] += new_quaternion[2] * rank weighted_average[6] += new_quaternion[3] * rank rank -= 1 if index > 0: del by_block_average[i] index += 1 by_block_average[g[0]][0] = weighted_average[0] * ratio by_block_average[g[0]][1] = weighted_average[1] * ratio by_block_average[g[0]][2] = weighted_average[2] * ratio by_block_average[g[0]][3] = weighted_average[3] * ratio by_block_average[g[0]][4] = weighted_average[4] * ratio by_block_average[g[0]][5] = weighted_average[5] * ratio by_block_average[g[0]][6] = weighted_average[6] * ratio for i in by_block_average: res.tag_poses.extend(by_block_average[i]) res.types.append(3 if i < 4 else (0 if i < 30 else (1 if i < 44 else (2 if i < 58 else 3)))) res.ids.append(i) return res
def plan_path(req): global planning_context planning_context.lock.acquire() try: move_group = planning_context.move_group scene = planning_context.scene arena = planning_context.arena state_mesh = planning_context.state_mesh robot = planning_context.robot robot_config = settings.ROBOT_CONFIG() scene.remove_world_object() #clear scene, needs syncing later move_group.clear_pose_targets() #if req.state_pose is different from current robot pose, apply it to planning joint_state = robot.get_current_state().joint_state if req.state_pose and len(req.state_pose) == 6 and not state_equals(joint_state.position, req.state_pose): joint_state.position = req.state_pose moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state move_group.set_start_state(moveit_robot_state) print "Planning path for session [" + req.session + "] from\n" + str(joint_state.position) + " to\n" + str(req.goal_pose) print "\n\033[1mDetected collisions mask:\033[0m" print "[self_collision, wall_collision, table_collision, state_collision, full_scene]" #default response res = ro_plan_pathResponse() res.collisions = [False, False, False, False, False] res.path = None #attempting to plan on empty scene, if this fails, it's due to self collision sync_scene('') res.path = move_group.plan(req.goal_pose).joint_trajectory if len(res.path.points) == 0: res.message = "self collision" res.collisions[0] = True res.collisions[4] = True #collision in full scene print str(res.collisions) time.sleep(0.1) return res #add walls to scene and replan expected_scene_objects = [] #needed for syncing for name in dir(arena): if name.startswith('pln'): obj = getattr(arena, name) obj.header.frame_id = settings.BASE_FRAME_ID_KINEMATICS scene.add_plane(name, obj) expected_scene_objects.append(name) sync_scene(expected_scene_objects) res.path = move_group.plan(req.goal_pose).joint_trajectory if len(res.path.points) == 0: res.message = "wall collision " res.collisions[1] = True res.collisions[4] = True #collision in full scene time.sleep(0.1) return res #clear scene, add table and replan scene.remove_world_object() expected_scene_objects = [] for name in dir(arena): if name.startswith('box'): obj = getattr(arena, name) obj["pose"].header.frame_id = settings.BASE_FRAME_ID_KINEMATICS scene.add_box(name, obj["pose"], obj["size"]) expected_scene_objects.append(name) sync_scene(expected_scene_objects) res.path = move_group.plan(req.goal_pose).joint_trajectory if len(res.path.points) == 0: res.message = "table collision " res.collisions[2] = True res.collisions[4] = True #collision in full scene time.sleep(0.1) return res # clear scene, add state mesh and replan scene.remove_world_object() expected_scene_objects = [] if len(req.state_mesh_vertices) > 0 and len(req.state_mesh_indices) > 0: mesh_pose = PoseStamped() mesh_pose.header.frame_id = settings.BASE_FRAME_ID_KINEMATICS mesh_pose.header.stamp = rospy.Time.now() mesh_pose.pose.orientation.w = 1.0 mesh = build_collision_mesh(req.state_mesh_vertices, req.state_mesh_indices) scene.add_mesh_custom('state_mesh', mesh_pose, mesh) expected_scene_objects.append('state_mesh') sync_scene(expected_scene_objects) res.path = move_group.plan(req.goal_pose).joint_trajectory if len(res.path.points) == 0: res.message = "state collision " res.collisions[3] = True res.collisions[4] = True #collision in full scene time.sleep(0.1) return res #if no previous collisions in seperated objects, add planes and boxes back to scene and retry for name in dir(arena): if name.startswith('pln'): obj = getattr(arena, name) obj.header.frame_id = settings.BASE_FRAME_ID_KINEMATICS scene.add_plane(name, obj) expected_scene_objects.append(name) elif name.startswith('box'): obj = getattr(arena, name) obj["pose"].header.frame_id = settings.BASE_FRAME_ID_KINEMATICS scene.add_box(name, obj["pose"], obj["size"]) expected_scene_objects.append(name) sync_scene(expected_scene_objects) traj = move_group.plan(req.goal_pose) if len(traj.joint_trajectory.points) == 0: res.message = "combination collision " res.collisions[4] = True #collision in full scene print str(res.collisions) time.sleep(0.1) return res speed = robot_config['jogging_speed'] if req.speed == None else req.speed traj = move_group.retime_trajectory(ref_state_in = robot.get_current_state(), traj_in = traj, velocity_scaling_factor = speed) res.path = traj.joint_trajectory res.message = "SUCCESS" print str(res.collisions) time.sleep(0.1) return res finally: planning_context.lock.release()
def plan_cartesian(req): global planning_context planning_context.lock.acquire() try: move_group = planning_context.move_group scene = planning_context.scene arena = planning_context.arena state_mesh = planning_context.state_mesh robot = planning_context.robot res = ro_plan_cartesianResponse() #remove any previous targets move_group.clear_pose_targets() robot_config = settings.ROBOT_CONFIG() way_points = partition_list(req.way_points) current_tcp = move_group.get_current_pose("extruder_tip_link").pose m_a = MarkerArray() m_a.markers.append(tcp_marker( [current_tcp.position.x, current_tcp.position.y, current_tcp.position.z, current_tcp.orientation.x, current_tcp.orientation.y, current_tcp.orientation.z, current_tcp.orientation.w], 'current_tcp', 0, 1.0,0.0, 0.3)) first_robot_state = copy.deepcopy(robot.get_current_state()) first_robot_state.joint_state.position = req.first_way_point_joint_states last_robot_state = copy.deepcopy(robot.get_current_state()) last_robot_state.joint_state.position = req.last_way_point_joint_states move_group.set_start_state(first_robot_state) mg_way_points = [] i = 0 for p in way_points: x = p[0] * 0.001 y = p[1] * 0.001 z = p[2] * 0.001 m_a.markers.append(tcp_marker( [x, y, z, p[3], p[4], p[5], p[6]], 'print_path', i, float(i) / len(way_points), 1.0 - (float(i) / len(way_points)), 1.0)) m_wp = copy.deepcopy(current_tcp) m_wp.position.x = x m_wp.position.y = y m_wp.position.z = z m_wp.orientation.x = p[3] m_wp.orientation.y = p[4] m_wp.orientation.z = p[5] m_wp.orientation.w = p[6] mg_way_points.append(m_wp) i += 1 #fk_res = srv_proxy_fk.call(header = Header(), fk_link_names = ["extruder_tip_link"], robot_state = last_robot_state) #mg_way_points.append(fk_res.pose_stamped[0].pose) marker_pub_path.publish(m_a) #moveit_robot_state.joint_state = way_points[0] (moveit_trajectory, fraction) = move_group.compute_cartesian_path(mg_way_points, robot_config['eef_step'], robot_config['jump_threshold']) print "Cartesian motion planned to " + str(fraction*100.0) + "%" speed = robot_config['print_speed'] if req.speed == None else req.speed moveit_trajectory = move_group.retime_trajectory(robot.get_current_state(), moveit_trajectory, robot_config['print_speed']) m_t = Marker() m_t.ns = "motion-trajectory" m_t.id = 0 m_t.header.frame_id = '/world' m_t.header.stamp = rospy.Time.now() m_t.type=Marker.POINTS m_t.action = Marker.ADD m_t.scale.x = 0.0005 m_t.color.a = 1.0 tmp_state = robot.get_current_state() for t in moveit_trajectory.joint_trajectory.points: #draw path points tmp_state.joint_state.position = t.positions fk_res = srv_proxy_fk.call(header=Header(), robot_state = tmp_state, fk_link_names = ["extruder_tip_link"]) m_t.points.append( Point( x=fk_res.pose_stamped[0].pose.position.x, y=fk_res.pose_stamped[0].pose.position.y, z=fk_res.pose_stamped[0].pose.position.z)) m_a.markers.append(m_t) marker_pub_path.publish(m_a) res.message = 'SUCCESS' if fraction == 1.0 else 'fraction: ' + str(fraction) res.motion_plan = moveit_trajectory.joint_trajectory res.collisions = [] return res finally: planning_context.lock.release()
def print_path(req): global DISABLE_USER_QUERY_MOVE print colored( "[print_path] Received print request. Start motion planning...", 'cyan') res = ro_print_pathResponse() robot_config = settings.ROBOT_CONFIG() first_way_points_in_layer = pp.partition_list( req.first_way_point_joint_states, partition_size=6) last_way_points_in_layer = pp.partition_list( req.last_way_point_joint_states, partition_size=6) way_point_layers = [] top = 0 for w in req.num_way_points: _from = top _to = w * 7 + top layer = req.way_points_cartesian[_from:_to] top += w * 7 way_point_layers.append(layer) num_layers = len(way_point_layers) tol = 0.1 #threshold for which joint state transitions btw action and first and last way points are considered safe and free of axis flips if not pp.state_equals( req.action, req.first_way_point_joint_states, tolerance=tol) or not pp.state_equals( req.action, req.last_way_point_joint_states, tolerance=tol): _in = raw_input( "Joint states of action, first way point and last way point diverge a lot. Double check for correct axis toggles. Continue?[y/n]" ) if _in != 'y': res.message = "User interrupt" return res #plan path from current to action current_to_action = srv_proxy_path.call(goal_pose=req.action, session="print_planning") if current_to_action.message != 'SUCCESS' or not pp.state_equals( req.action, current_to_action.path.points[-1].positions, tolerance=0.005): res.message = "Couldn't move from current state to action: " + ( current_to_action.message if current_to_action.message != 'SUCCESS' else "Final state deviates too far") return res action_to_firsts = [] print_paths = [] last_to_actions = [] #everything layer specific for i in range(num_layers): #plan path from action to first way point first_way_point_joint_states = first_way_points_in_layer[i] last_way_point_joint_states = last_way_points_in_layer[i] way_points_cartesian = way_point_layers[i] action_to_first = srv_proxy_path.call( state_pose=req.action, goal_pose=first_way_point_joint_states, session="print_planning") if action_to_first.message != 'SUCCESS' or not pp.state_equals( req.action, action_to_first.path.points[0].positions, tolerance=0.005): res.message = "Layer " + str( i) + ": Couldn't move to from action to first way point: " + ( action_to_first.message if action_to_first.message != 'SUCCESS' else "Jump between last path segment and this one") return res action_to_firsts.append(copy.deepcopy(action_to_first)) #plan cartesian print path print_path = srv_proxy_cartesian.call( first_way_point_joint_states=first_way_point_joint_states, last_way_point_joint_states=last_way_point_joint_states, way_points=way_points_cartesian) if print_path.message != "SUCCESS" or not pp.state_equals( print_path.motion_plan.points[0].positions, action_to_first.path.points[-1].positions, tolerance=0.005): res.message = "Layer " + str( i) + ": Couldn't find cartesian print path: " + ( print_path.message if print_path.message != "SUCCESS" else "Jump between last path segment and this one") return res print_paths.append(copy.deepcopy(print_path)) #plan from last way point back to action last_to_action = srv_proxy_path.call( state_pose=last_way_point_joint_states, goal_pose=req.action, session="print_planning") if last_to_action.message != 'SUCCESS' or not pp.state_equals( last_to_action.path.points[0].positions, print_path.motion_plan.points[-1].positions, tolerance=0.005): res.message = "Layer " + str( i) + ": Couldn't move to from last way point to action: " + ( last_to_action.message if last_to_action.message != "SUCCESS" else "Jump between last path segment and this one") return res last_to_actions.append(copy.deepcopy(last_to_action)) #plan from action back to current action_to_current = srv_proxy_path.call( state_pose=req.action, goal_pose=current_to_action.path.points[0].positions, session="print_planning") if action_to_current.message != 'SUCCESS' or not pp.state_equals( action_to_current.path.points[0].positions, last_to_action.path.points[-1].positions, tolerance=0.005): res.message = "Layer " + str( i) + ": Couldn't move from action to current: " + ( action_to_current.message if action_to_current.message != "SUCCESS" else "Jump between last path segment and this one") return res print colored("[print_path] ...Motion planning success.", 'cyan') total_time = current_to_action.path.points[-1].time_from_start for i in range(num_layers): total_time += action_to_firsts[i].path.points[-1].time_from_start \ + print_paths[i].motion_plan.points[-1].time_from_start \ + last_to_actions[i].path.points[-1].time_from_start total_time += action_to_current.path.points[-1].time_from_start _in = raw_input("[print_path] Double check print path! It will take " + str(total_time / 1000000000) + " secs to finish. Start?[y/n]")[0] if _in == 'n': res.message += ". User interrupt" return res mo_req = ro_move_pathRequest() mo_req.wait = True #move to action DISABLE_USER_QUERY_MOVE = True mo_req.path = current_to_action.path mo_res = move_path(mo_req) #set motor to ready set via reset and start srv_proxy_reset_motor.call() srv_proxy_start_motor.call() #start heating nozzle, wait for it to reach temp srv_proxy_change_temp.call(value=robot_config['print_temp'], wait=True) #speed up motor, wait a bit srv_proxy_change_motor_speed.call(value=robot_config["print_override"] / 3) time.sleep(5.0) srv_proxy_change_motor_speed.call(value=robot_config["print_override"] * 2 / 3) time.sleep(5.0) srv_proxy_change_motor_speed.call(value=robot_config["print_override"]) _in = raw_input("Check good material flow press Enter") srv_proxy_change_motor_speed.call(value=0.0) _in = raw_input("Press Enter to start print") for i in range(num_layers): try: robot_config = settings.ROBOT_CONFIG() srv_proxy_change_temp.call(value=robot_config['print_temp'], wait=True) #move to layer start print colored( "[print_path] Printing layer " + str(i) + " of " + str(num_layers), 'cyan') DISABLE_USER_QUERY_MOVE = True mo_req.path = action_to_firsts[i].path mo_res = move_path(mo_req) #speed up motor and wait a bit srv_proxy_change_motor_speed.call( value=robot_config["print_override"]) time.sleep(1) #move along print path DISABLE_USER_QUERY_MOVE = True mo_req.path = print_paths[i].motion_plan mo_res = move_path(mo_req) #slow motor down again and wait a bit srv_proxy_change_motor_speed.call( value=robot_config["default_override"]) time.sleep(1) #move to action DISABLE_USER_QUERY_MOVE = True mo_req.path = last_to_actions[i].path mo_res = move_path(mo_req) except KeyboardInterrupt: _in = raw_input( "[print_path] Keyboard interrupt. Continue print?[y/n]") if _in == 'y': pass else: res.message = "User interrupt" return res #start cooling down srv_proxy_change_temp.call(value=robot_config['default_temp'], wait=False) #move back home DISABLE_USER_QUERY_MOVE = True mo_req.path = action_to_current.path mo_res = move_path(mo_req) move_path_msg = mo_res.message print colored("[print_path] ...Move path result: " + move_path_msg, 'cyan') res.message += ". move path msg: " + move_path_msg return res