def active_tilt(point, axis, angle, velocity, active_distance=0.125):
'''Tilt primitive motion of robot.
Parameters:
point (list): 3-D coordinate of point in rotation axis
axis (list): 3-D vector of rotation axis (right-hand rule)
angle (double): angle of tilting
velocity (double): robot velocity between 0 and 1
Returns:
'''
# Normalize axis vector
axis = axis / np.linalg.norm(axis)
# Pose variables. The parameters can be seen from "$ rosmsg show Pose"
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
]
# Tilt center point. Closest point from tcp to axis line
center = np.add(point,
np.dot(np.subtract(pos_initial, point), axis) * axis)
# Closest distance from tcp to axis line
radius = np.linalg.norm(np.subtract(center, pos_initial))
# Pair of orthogonal vectors in tilt plane
v1 = -np.subtract(
np.add(center,
np.dot(np.subtract(pos_initial, center), axis) * axis),
pos_initial)
v1 = v1 / np.linalg.norm(v1)
v2 = np.cross(axis, v1)
# 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))
waypoints = []
for t in range(1, angle + 1):
circle = np.add(
center,
radius * (math.cos(math.radians(t))) * v1 + radius *
(math.sin(math.radians(t))) * v2)
pose_target.position.x = circle[0]
pose_target.position.y = circle[1] + active_distance * t / (angle + 1)
pose_target.position.z = circle[2]
pose_target.orientation.x = ori_waypoints[t - 1][0]
pose_target.orientation.y = ori_waypoints[t - 1][1]
pose_target.orientation.z = ori_waypoints[t - 1][2]
pose_target.orientation.w = ori_waypoints[t - 1][3]
waypoints.append(copy.deepcopy(pose_target))
(plan, fraction) = group.compute_cartesian_path(
waypoints, 0.01, 0) # waypoints, resolution=1cm, jump_threshold)
retimed_plan = group.retime_trajectory(
robot.get_current_state(), plan,
velocity) # Retime trajectory with scaled velocity
group.execute(retimed_plan)
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(robot, axis, angle, psi_init=0, acc=0.1, vel=0.1, wait=True):
with open(
"/home/john/Desktop/dexterous_ungrasping_urx/config/du_config.yaml",
'r') as stream:
try:
config = yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
tcp2fingertip = config['tcp2fingertip']
init_pose = robot.get_pose()
contact_A_e = m3d.Vector(
config['object_thickness'] / 2, 0, tcp2fingertip
) #TODO: depends on axis direction and gripper orientation. Refer to gripper axis for now and adjust accordingly.
contact_A_w = init_pose * contact_A_e
# Interpolate orientation poses via quaternion slerp
q_initial = helper.matrix2quaternion(init_pose.orient)
q = helper.axis_angle2quaternion(axis, angle)
q_target = tf.transformations.quaternion_multiply(q, q_initial)
q_waypoints = helper.slerp(
q_initial, q_target,
np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle))
theta_0 = config['theta_0']
waypoints = []
pose_via = init_pose.copy()
for psi in range(psi_init + 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_via.pos[1] = contact_A_w[1] + hori
pose_via.pos[2] = contact_A_w[2] + verti
#print "CASE 1"
elif axis[0] < 0:
pose_via.pos[1] = contact_A_w[1] - hori
pose_via.pos[2] = contact_A_w[2] + verti
#print "CASE 2"
elif axis[1] > 0:
pose_via.pos[0] = contact_A_w[0] - hori
pose_via.pos[2] = contact_A_w[2] + verti
#print "CASE 3"
elif axis[1] < 0:
pose_via.pos[0] = contact_A_w[0] + hori
pose_via.pos[2] = contact_A_w[2] + verti
#print "CASE 4"
pose_via.orient = tf.transformations.quaternion_matrix(
q_waypoints[psi - psi_init - 1])[:3, :3]
waypoints.append(copy.deepcopy(pose_via))
#print init_pose
#print waypoints
robot.movexs(command='movel',
pose_list=waypoints,
acc=acc,
vel=vel,
radius=0.005,
wait=False)
psi = 0
while psi < angle:
pose_current = robot.get_pose()
q_current = helper.matrix2quaternion(pose_current.orient)
psi = 2 * math.degrees(math.acos(np.dot(q_current, q_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 + 0.004
Robotiq.goto(robotiq_client,
pos=width,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
psi = round(psi, 2)
rospy.sleep(0.5)
return width
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
