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