def home():
select = request.form.get('co-select')
select_text = ""
ret = 'Select Data'
vis = visualizer()
if select == 'fbf':
ret = vis.visualizeFbFeed()
elif select == 'fba':
ret = vis.visualizeFbAll()
elif select == 'apf':
ret = vis.visualizeApFeed()
elif select == 'apa':
ret = vis.visualizeApAll()
elif select == 'amf':
ret = vis.visualizeAmFeed()
elif select == 'ama':
ret = vis.visualizeAmAll()
elif select == 'nef':
ret = vis.visualizeNfFeed()
elif select == 'nea':
ret = vis.visualizeNfAll()
elif select == 'gof':
ret = vis.visualizeGoFeed()
elif select == 'goa':
ret = vis.visualizeGoAll()
return render_template('home.html', title=select_text, plot=ret)
# Visualize object during regrasp
P_g_center_viz = [tcp2fingertip + object_length - delta_0, 0, 0, 1]
P_w_center_viz = np.matmul(T_wg, P_g_center_viz)
center_viz = P_w_center_viz[:3]
p = group.get_current_pose().pose
object_v = [
center_viz[0] - p.position.x, center_viz[1] - p.position.y,
center_viz[2] - p.position.z
]
object_uv = object_v / np.sum(np.power(object_v, 2))**0.5
object_edge = np.multiply(object_uv, object_length)
visualization.thin_object(center_viz,
np.subtract(center_viz, object_edge),
object_thickness, 3)
visualization.visualizer(np.subtract(center_viz, object_edge), 1, 0.01,
4)
# Regrasp
rospy.sleep(0.5)
regrasp.palm_regrasp(np.multiply(axis, -1), int(psi_regrasp),
tcp_speed)
rospy.sleep(0.5)
# Tilt
tilt.tilt(center, axis, int(theta_tilt), tcp_speed)
rospy.sleep(0.5)
# Surface Slide
motion_primitives.set_pose_relative([0, 0, 0.01])
rospy.sleep(0.5)
def test_visualization(X_test, y_test):
model = keras.models.load_model("best_models/best_model-462-0.01.hdf5",
custom_objects={"dice_coef": dice_coef})
visualizer(model)
# Set TCP speed
group.set_max_velocity_scaling_factor(tcp_speed)
# Set gripper position
Robotiq.goto(robotiq_client,
pos=object_thickness,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
center = [
pose[0], pose[1], pose[2] - tcp2fingertip - object_length + delta_0
]
visualization.visualizer(position=center,
marker_type=1,
scale=0.01,
id=2)
# Tilt
tilt.tilt(center, axis, int(90 - theta_0), tcp_speed)
# Visualize object during regrasp
p = group.get_current_pose().pose
object_v = [
center[0] - p.position.x, center[1] - p.position.y,
center[2] - p.position.z
]
object_uv = object_v / np.sum(np.power(object_v, 2))**0.5
object_edge = np.multiply(object_uv, object_length)
visualization.thin_object(center, np.subtract(center, object_edge),
object_thickness, 3)
motion_primitives.set_pose(init_pose)
p = group.get_current_pose().pose
'''
#TEST GRIPPER LENGTH
pos_initial = [p.position.x, p.position.y, p.position.z]
ori_initial = [p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w]
T_we = tf.TransformListener().fromTranslationRotation(pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
contact_A_e = [tcp2fingertip, -object_thickness/2, 0, 1]
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.005, 2) #DEBUG
#TEST GRIPPER LENGTH
'''
center = [
p.position.x, p.position.y,
p.position.z - tcp2fingertip - object_length + delta_0
]
visualization.visualizer(center, "s", 0.01, 0)
rospy.sleep(0.5)
tilt.tilt(center, axis, int(90 - theta_0), tcp_speed)
regrasp.regrasp(np.multiply(axis, -1), int(psi_regrasp), tcp_speed)
tilt.tilt(center, axis, int(theta_tilt), tcp_speed)
tuck.rotate_tuck(np.multiply(axis, -1), int(theta_0 - theta_tilt),
0.03, tcp_speed)
rospy.spin()
except rospy.ROSInterruptException:
pass
def regrasp(axis, angle, velocity):
with open("/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml", 'r') as stream:
try:
config = yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
pose_target = group.get_current_pose().pose
pos_initial = [pose_target.position.x, pose_target.position.y, pose_target.position.z]
ori_initial = [pose_target.orientation.x, pose_target.orientation.y, pose_target.orientation.z, pose_target.orientation.w]
T_we = tf.TransformListener().fromTranslationRotation(pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
contact_A_e = [tcp2fingertip, -config['object_thickness']/2, 0, 1] #TODO: depends on axis direction
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG
# Interpolate orientation poses via quaternion slerp
q = helper.axis_angle2quaternion(axis, angle)
ori_target = tf.transformations.quaternion_multiply(q, ori_initial)
ori_waypoints = helper.slerp(ori_initial, ori_target, np.arange(1.0/angle , 1.0+1.0/angle, 1.0/angle))
theta_0 = config['theta_0']
waypoints = []
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction)
for psi in range(1, angle+1):
# Calculate width
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
# Calculate position
if theta_0 + psi <= 90:
hori = math.fabs(tcp2fingertip*math.cos(math.radians(theta_0 + psi))) + math.fabs((width/2.0)*math.sin(math.radians(theta_0+psi)))
verti = math.fabs(tcp2fingertip*math.sin(math.radians(theta_0 + psi))) - math.fabs((width/2.0)*math.cos(math.radians(theta_0+psi)))
else:
hori = -math.fabs(tcp2fingertip*math.sin(math.radians(theta_0 + psi-90))) + math.fabs((width/2.0)*math.cos(math.radians(theta_0+psi-90)))
verti = math.fabs(tcp2fingertip*math.cos(math.radians(theta_0 + psi-90))) + math.fabs((width/2.0)*math.sin(math.radians(theta_0+psi-90)))
if axis[0] > 0:
pose_target.position.y = contact_A_w[1] + hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 1"
elif axis[0] < 0:
pose_target.position.y = contact_A_w[1] - hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 2"
elif axis[1] > 0:
pose_target.position.x = contact_A_w[0] - hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 3"
elif axis[1] < 0:
pose_target.position.x = contact_A_w[0] + hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 4"
pose_target.orientation.x = ori_waypoints[psi-1][0]
pose_target.orientation.y = ori_waypoints[psi-1][1]
pose_target.orientation.z = ori_waypoints[psi-1][2]
pose_target.orientation.w = ori_waypoints[psi-1][3]
waypoints.append(copy.deepcopy(pose_target))
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
retimed_plan = group.retime_trajectory(robot.get_current_state(), plan, velocity)
group.execute(retimed_plan, wait=False)
opening_at_zero = config['max_opening']-2*config['finger_thickness']
psi = 0
while psi < angle:
pose = group.get_current_pose().pose
q_current = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
psi = 2*math.degrees(math.acos(np.dot(q_current, ori_initial)))
if psi > 100:
psi = -(psi-360)
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
#pos = int((opening_at_zero - width)/config['opening_per_count'])
Robotiq.goto(robotiq_client, pos=width, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
psi = round(psi, 2)
def regrasp(axis, angle, velocity):
with open(
"../config/Go_stone_du_config.yaml", 'r'
) as stream: # This saves the relative config between the obj and the gripper
try:
config = yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
pose_target = group.get_current_pose().pose # tcp is at wrist
pos_initial = [
pose_target.position.x, pose_target.position.y, pose_target.position.z
]
ori_initial = [
pose_target.orientation.x, pose_target.orientation.y,
pose_target.orientation.z, pose_target.orientation.w
]
T_we = tf.TransformListener().fromTranslationRotation(
pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
contact_A_e = [tcp2fingertip, config['object_thickness'] / 2 + 0.00, 0,
1] #TODO: depends on axis direction
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG
# Interpolate orientation poses via quaternion slerp
q = helper.axis_angle2quaternion(axis, angle)
ori_target = tf.transformations.quaternion_multiply(
q, ori_initial) # target orientation of the gripper
ori_waypoints = helper.slerp(
ori_initial, ori_target,
np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle))
theta_0 = config['theta_0']
waypoints = []
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(
action_name, CommandRobotiqGripperAction
) # this is for new robotiq gripper control package
for psi in range(1, angle + 1): # psi is the angle to regrasp
# Calculate width
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
# Calculate position
if theta_0 + psi <= 90: # hori (verti) is the horizontal (vertical) distance between ee_link and contact_A, in base_link frame
hori = math.fabs(tcp2fingertip * math.cos(
math.radians(theta_0 + psi))) + math.fabs(
(width / 2.0) * math.sin(math.radians(theta_0 + psi)))
verti = math.fabs(tcp2fingertip * math.sin(
math.radians(theta_0 + psi))) - math.fabs(
(width / 2.0) * math.cos(math.radians(theta_0 + psi)))
else:
hori = -math.fabs(tcp2fingertip * math.sin(
math.radians(theta_0 + psi - 90))) + math.fabs(
(width / 2.0) * math.cos(math.radians(theta_0 + psi - 90)))
verti = math.fabs(tcp2fingertip * math.cos(
math.radians(theta_0 + psi - 90))) + math.fabs(
(width / 2.0) * math.sin(math.radians(theta_0 + psi - 90)))
if axis[0] > 0: # axis is always orthorgonal to base_link frame [1 0 0] or [-1 0 0] or [0 1 0] or [0 -1 0]
pose_target.position.y = contact_A_w[
1] + hori # ee_link position in base_link frame
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 1"
elif axis[0] < 0:
pose_target.position.y = contact_A_w[1] - hori - (
0.085 / (25 * angle / 37)) * psi / angle
pose_target.position.z = contact_A_w[2] + verti + (
0.1 / (25 * angle / 37)) * psi / angle
#print "CASE 2"
elif axis[1] > 0:
pose_target.position.x = contact_A_w[0] - hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 3"
elif axis[1] < 0:
pose_target.position.x = contact_A_w[0] + hori
pose_target.position.z = contact_A_w[2] + verti
print "CASE 4"
pose_target.orientation.x = ori_waypoints[psi - 1][
0] # ee_link orientation in base_link frame
pose_target.orientation.y = ori_waypoints[psi - 1][1]
pose_target.orientation.z = ori_waypoints[psi - 1][2]
pose_target.orientation.w = ori_waypoints[psi - 1][3]
waypoints.append(copy.deepcopy(pose_target))
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
print "Executing regrasp motion."
retimed_plan = group.retime_trajectory(robot.get_current_state(), plan,
velocity)
group.execute(retimed_plan, wait=False)
opening_at_zero = config['max_opening'] - 2 * config['finger_thickness']
psi = 0
while psi < angle: # while the manipulator is moving, also need to compute the gripper width simultaneously
pose = group.get_current_pose().pose
q_current = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
psi = 2 * math.degrees(
math.acos(np.dot(q_current, ori_initial))
) #first get psi, and then compute a b c d. During this time, the manipulator keeps
if psi > 100:
psi = -(psi - 360)
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
Robotiq.goto(
robotiq_client,
pos=width + 0.004,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False
) #0.006 for coin; 0.000 for book; 0.005 for poker; 0.007 for flat surface; 0.0065 for curved surface
psi = round(psi, 2)
rospy.sleep(0.5)
print width + 0.003
return width + 0.003
