def grab_object(distance):
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction)
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_relative([0, 0, -distance])
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_relative([0, 0, distance])
def operate_gripper():
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction)
# Wait until grippers are ready to take command
robotiq_client.wait_for_server()
rospy.logwarn("Client test: Starting sending goals")
## Manually set all the parameters of the gripper goal state.
######################################################################################
goal = CommandRobotiqGripperGoal()
goal.emergency_release = False
goal.stop = False
goal.position = 0.00 #/m
goal.speed = 0.1 #m/s
goal.force = 5.0 #0-100%
# Sends the goal to the gripper.
robotiq_client.send_goal(goal)
# Block processing thread until gripper movement is finished, comment if waiting is not necesary.
robotiq_client.wait_for_result()
# Use pre-defined functions for robot gripper manipulation.
#####################################################################################
while not rospy.is_shutdown():
Robotiq.goto(robotiq_client, pos=0.00, speed=0.1, force=100 , block=True)
Robotiq.goto(robotiq_client, pos=0.04, speed=0.01, force=10)
Robotiq.goto(robotiq_client, pos=0.011, speed=0.01, force=0 , block=True)
Robotiq.goto(robotiq_client, pos=0.08, speed=0.11, force=200 , block=True)
# Robotiq.goto(robotiq_client, pos=0.06, speed=0.0, force=0)
# break
# Prints out the result of executing the action
return robotiq_client.get_result() # A FibonacciResult
def second_regrasp(axis, angle, pos, 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)
tcp2fingertip = config['tcp2fingertip']
p = group.get_current_pose().pose
trans_tool0 = [p.position.x, p.position.y, p.position.z]
rot_tool0 = [
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w
]
T_wg = tf.TransformerROS().fromTranslationRotation(trans_tool0, rot_tool0)
P_g_center = [tcp2fingertip + 0.02, -pos / 2.0, 0, 1]
P_w_center = np.matmul(T_wg, P_g_center)
center = [P_w_center[0], P_w_center[1], P_w_center[2]]
waypoints = tilt.tilt_no_wait(center, axis, int(angle), velocity)
#print waypoints
rospy.sleep(0.5)
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
current_angle = 0
while current_angle < angle:
pose = group.get_current_pose().pose
q_current = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
current_angle = 2 * math.degrees(
math.acos(np.dot(q_current, rot_tool0)))
if current_angle > 100:
current_angle = -(psi - 360)
Robotiq.goto(robotiq_client,
pos=pos + 0.00315 + 0.012 * (current_angle / angle),
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
current_angle = round(current_angle, 2)
object_thickness = config['object_thickness']
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)
pre_pick_pose = [-0.092, 0.592, 0.416, 0.7071, -0.0, -0.7071, 0.0]
pick_pose = [-0.092, 0.592, 0.282, 0.7071, -0.0, -0.7071, 0.0]
init_pose=[-0.344, 0.538, 0.458, -0.7071, -0.0, 0.7071, 0.0]
regrasp_pose=[-0.344, 0.538, 0.350, -0.7071, -0.0, 0.7071, 0.0]
Robotiq.goto(robotiq_client, pos=0.04, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
rospy.sleep(0.5)
motion_primitives.set_pose(pre_pick_pose)
rospy.sleep(0.5)
motion_primitives.set_pose(pick_pose)
rospy.sleep(1)
Robotiq.goto(robotiq_client, pos=object_thickness+0.002, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
rospy.sleep(1)
motion_primitives.set_pose(pre_pick_pose)
#Robotiq.goto(robotiq_client, pos=object_thickness+0.003, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
motion_primitives.set_pose(regrasp_pose)
#for i in range(11):
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
# Wait until grippers are ready to take command
robotiq_client.wait_for_server()
#rospy.logwarn("Client test: Starting sending goals")
## Manually set all the parameters of the gripper goal state.
'''
goal = CommandRobotiqGripperGoal()
goal.emergency_release = False
goal.stop = False
goal.position = 50
goal.speed = 10
goal.force = 5.0
# Sends the goal to the gripper.
robotiq_client.send_goal(goal)
# Block processing thread until gripper movement is finished, comment if waiting is not necesary.
robotiq_client.wait_for_result()
'''
# Use pre-defined functions for robot gripper manipulation.
Robotiq.goto(robotiq_client, pos=0.04, speed=10, force=5, block=True)
print "HI"
#rospy.sleep(0.1)
#Robotiq.goto(robotiq_client, pos=190, speed=20, force=5 , block=True)
#rospy.sleep(0.1)
#Robotiq.goto(robotiq_client, pos=180, speed=100, force=5 , block=True)
#rospy.sleep(0.1)
#Robotiq.goto(robotiq_client, pos=170, speed=40, force=5 , block=True)
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)
pre_pick_pose = [-0.178, -0.492, 0.416, 0.5, -0.5, -0.5, 0.5]
pick_pose = [-0.078, 0.592, 0.302, 0.7071, -0.0, -0.7071, 0.0]
init_pose = [-0.344, 0.538, 0.458, -0.7071, -0.0, 0.7071, 0.0]
regrasp_pose = [0.344, -0.538, 0.346, 0.7071, -0.0, -0.7071, 0.0]
Robotiq.goto(robotiq_client,
pos=0.011,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
rospy.sleep(0.5)
# pick and place
motion_primitives.set_joint(
[151.7, -67.32, -136.40, -67.59, 90.76, -28.48])
rospy.sleep(1)
motion_primitives.set_pose_relative([0, 0, 0.0025])
servo_pub.publish(160)
rospy.sleep(1)
dynamixel.set_position(230)
rospy.sleep(2)
dynamixel.set_position(100)
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
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
# Wait until grippers are ready to take command
robotiq_client.wait_for_server()
#rospy.logwarn("Client test: Starting sending goals")
## Manually set all the parameters of the gripper goal state.
'''
goal = CommandRobotiqGripperGoal()
goal.emergency_release = False
goal.stop = False
goal.position = 50
goal.speed = 10
goal.force = 5.0
# Sends the goal to the gripper.
robotiq_client.send_goal(goal)
# Block processing thread until gripper movement is finished, comment if waiting is not necesary.
robotiq_client.wait_for_result()
'''
# Use pre-defined functions for robot gripper manipulation.
Robotiq.goto(robotiq_client, pos=200, speed=10, force=5, block=True)
rospy.sleep(0.1)
Robotiq.goto(robotiq_client, pos=190, speed=20, force=5, block=True)
rospy.sleep(0.1)
Robotiq.goto(robotiq_client, pos=180, speed=100, force=5, block=True)
rospy.sleep(0.1)
Robotiq.goto(robotiq_client, pos=170, speed=40, force=5, block=True)
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
