def push_tuck(axis, angle, fingertip2contactB, velocity, tuck):
'''Rotate tuck primitive motion of robot.
Parameters:
axis (list): 3-D vector of rotation axis (right-hand rule)
angle (double): angle of tucking
fingertip2contactB (double): distance from fingertip to contact B in meters
velocity (double): robot velocity between 0 and 1
Returns:
'''
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)
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
# TODO: use pjg module
#msg = rospy.wait_for_message('/CModelRobotInput', inputMsg.CModel_robot_input, timeout = None)
#gripper_position = msg.gPO
gripper_position = 255 #TEMP DEBUG
#Robotiq.get_current_gripper_status(Robotiq())
#Robotiq.goto(robotiq_client, pos=object_thickness, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
# gripper kinematics
opening_at_zero = config['max_opening'] - 2 * config['finger_thickness']
gripper_opening = -config[
'opening_per_count'] * gripper_position + opening_at_zero
contact_B_e = [
config['tcp2fingertip'] - fingertip2contactB, -0.035 / 2.0, 0, 1
]
contact_B_w = np.matmul(T_we, contact_B_e)
dynamixel.set_length(tuck)
tilt.translate_tilt(contact_B_w[:3], axis, angle, velocity, 0.00)
sim = config['sim']
table_height_wrt_world = -0.02
# Set TCP speed
group.set_max_velocity_scaling_factor(tcp_speed)
init_pose = [-0.344, 0.538, 0.458, -0.7071, -0.0, 0.7071, 0.0]
prepick_pose = [-0.2613, 0.6531, 0.3442, -0.7071, -0.0, 0.7071, 0.0]
pick_pose = [-0.2613, 0.6531, 0.2924, -0.7071, -0.0, 0.7071, 0.0]
tilt_pose = [-0.341, 0.413, 0.359, -0.653, -0.271, 0.653, 0.271]
#prior_pose=[-0.433, 0.48, 0.525, 0.653, 0.271, 0.653, 0.271]
#initial_pose=[-0.433, 0.510, 0.534, 0.653, 0.271, 0.653, 0.271]
prior_pose = [-0.432, 0.4, 0.375, 0.653, 0.271, 0.653, 0.271]
initial_pose = [-0.432, 0.485, 0.405, 0.653, 0.271, 0.653, 0.271]
dynamixel.set_length(115)
#Robotiq.goto(robotiq_client, pos=object_thickness+0.015, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
#rospy.sleep(0.5)
#motion_primitives.set_pose(init_pose)
#motion_primitives.set_pose(prepick_pose)
#motion_primitives.set_pose(pick_pose)
Robotiq.goto(robotiq_client,
pos=object_thickness + 0.006,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
rospy.sleep(0.5)
#motion_primitives.set_pose(prepick_pose)
#motion_primitives.set_pose(tilt_pose)
motion_primitives.set_pose(prior_pose)
rospy.sleep(10)
object_length = config['object_length']
tcp2fingertip = config['tcp2fingertip']
sim = config['sim']
table_height_wrt_world = -0.02
# Set TCP speed
group.set_max_velocity_scaling_factor(tcp_speed)
init_pose = [-0.344, 0.538, 0.458, -0.7071, -0.0, 0.7071, 0.0]
prepick_pose = [-0.2613, 0.6531, 0.3442, -0.7071, -0.0, 0.7071, 0.0]
pick_pose = [-0.2613, 0.6531, 0.2924, -0.7071, -0.0, 0.7071, 0.0]
tilt_pose = [-0.341, 0.413, 0.359, -0.653, -0.271, 0.653, 0.271]
prior_pose = [-0.436, 0.427, 0.360, -0.653, -0.271, 0.653, 0.271]
initial_pose = [-0.432, 0.407, 0.576, -0.653, -0.271, 0.653, 0.271]
regrasp_pose = [-0.432, 0.462, 0.544, -0.653, -0.271, 0.653, 0.271]
dynamixel.set_length(120)
#Robotiq.goto(robotiq_client, pos=object_thickness+0.015, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
#rospy.sleep(0.5)
#motion_primitives.set_pose(init_pose)
#motion_primitives.set_pose(prepick_pose)
#motion_primitives.set_pose(pick_pose)
Robotiq.goto(robotiq_client,
pos=object_thickness + 0.002,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
rospy.sleep(0.5)
#motion_primitives.set_pose(prepick_pose)
#motion_primitives.set_pose(tilt_pose)
#motion_primitives.set_pose(prior_pose)
motion_primitives.set_pose(initial_pose)
def inverted_palm_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 # TODO: The only thing I changed for inverted palm regrasp is the (-) sign
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
palm_position = 127 + (config['delta_0'] - a) * 1000
#pos = int((opening_at_zero - width)/config['opening_per_count'])
Robotiq.goto(robotiq_client,
pos=width + 0.003,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
dynamixel.set_length(palm_position + 9)
psi = round(psi, 2)
