def lookup_transform(to_frame, baxter, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
# if to_frame =='pawn':
# to_frame = 'ar_marker_9'
if not baxter:
print( 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.')
tag_pos = [0.642, -0.132, -0.023]
tag_rot = [-0.001, -0.030, 0.024, 0.999]
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
#return RigidTransform(translation = tag_pos, to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
tag_pos, tag_rot = listener.lookupTransform(
from_frame, to_frame, t)
break
except:
rate.sleep()
attempts += 1
# tag_pos = [0.642, -0.132, -0.023]
# tag_rot = [-0.001, -0.030, 0.024, 0.999]
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t)
except:
rate.sleep()
attempts += 1
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot, tag_pos, to_frame=from_frame, from_frame=to_frame)
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
try:
# print("in try")
# print("1 here")
t = listener.getLatestCommonTime(from_frame, to_frame)
# print("here")
tag_pos, tag_rot = listener.lookupTransform(
from_frame, to_frame, t)
real_rot = np.array(
[tag_rot[3], tag_rot[0], tag_rot[1], tag_rot[2]])
# real_rot = np.array([tag_rot[1], tag_rot[2], tag_rot[3], tag_rot[0]])
break
except:
rate.sleep()
attempts += 1
# print("t",t)
# print("tag_pos", tag_pos)
# # print("tag_rot", tag_rot)
# print("real_rot", real_rot)
### comment out when using ar tag
# tag_rot = [0, 0, 1, 0]
# tag_pos = [0.570, -0.184, -.243] #changed this so it would work for right gripper better
### comment out when using ar tag
rot = RigidTransform.rotation_from_quaternion(real_rot)
# rot = tag_rot
# print(from_frame, to_frame)
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
def object_pose_from_json(scene_object_json):
# type: (dict) -> (np.array, np.array)
"""
Parses object pose from "location" and "quaternion_xyzw".
:param scene_object_json: JSON fragment of a single scene object
:return (translation, rotation) in meters
"""
quaternion_xyzw = np.array(scene_object_json['quaternion_xyzw'])
quaternion_wxyz = np.roll(quaternion_xyzw, 1)
rotation = RigidTransform.rotation_from_quaternion(quaternion_wxyz)
translation = np.array(scene_object_json['location'])
return translation, rotation
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
# Sleep for a bit before looking up transformation
for _ in range(1):
rate.sleep()
while attempts < max_attempts:
try:
# import pdb; pdb.set_trace()
t = listener.getLatestCommonTime(from_frame, to_frame)
# original
# tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t)
# changes from piazza
tag_pos, tag_rot = listener.lookupTransform(
to_frame, from_frame, t)
tag_rot = np.roll(tag_rot, 1)
break
except:
rate.sleep()
attempts += 1
print 'Attempt {0} to look up transformation from {1} to {2} unsuccessful.'.format(
attempts, from_frame, to_frame)
print 'Successfully found transformation from {0} to {1}.'.format(
from_frame, to_frame)
# import pdb; pdb.set_trace()
rot = RigidTransform.rotation_from_quaternion(tag_rot)
# original
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
def message_to_pose(message, from_frame='yumi'):
tokens = message.split()
try:
if len(tokens) != 7:
raise Exception("Invalid format for pose! Got:\n{0}".format(message))
pose_vals = [float(token) for token in tokens]
q = pose_vals[3:]
t = pose_vals[:3]
R = RigidTransform.rotation_from_quaternion(q)
pose = RigidTransform(R, t, from_frame=from_frame)
pose.position = pose.position * MM_TO_METERS
return pose
except Exception, e:
logging.error(e)
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
tag_rot = None
tag_pos = None
print('CALLING lookup_transform')
print('to_frame: {}, from_frame: {}'.format(to_frame, from_frame))
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
print('initializing transformlistener')
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
print('attempt {}'.format(attempts))
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
tag_pos, tag_rot = listener.lookupTransform(
from_frame, to_frame, t)
except Exception as e:
print(e)
rate.sleep()
attempts += 1
tag_rot = np.array([tag_rot[3], tag_rot[0], tag_rot[1], tag_rot[2]])
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot,
tag_pos,
to_frame=to_frame,
from_frame=from_frame)
def execute_grasp(T_grasp_world, planner, gripper, planned_path_pub,
goal_pos_pub):
"""
takes in the desired hand position relative to the object, finds the desired
hand position in world coordinates. Then moves the gripper from its starting
orientation to some distance BEHIND the object, then move to the hand pose
in world coordinates, closes the gripper, then moves up.
Parameters
----------
T_grasp_world : :obj:`autolab_core.RigidTransform`
desired position of gripper relative to the world frame
"""
def close_gripper():
"""closes the gripper"""
gripper.close(block=True)
rospy.sleep(1.0)
def open_gripper():
"""opens the gripper"""
gripper.open(block=True)
rospy.sleep(1.0)
inp = raw_input(
'Press <Enter> to move and open the gripper, or \'exit\' to exit')
if inp == "exit":
return
else:
open_gripper()
# # dummy position ######################################################
# quaternion = np.array([1, 0, 0, 0])
# dummy_rotation = RigidTransform.rotation_from_quaternion(quaternion)
# dummy_translation = np.array([0.1, 0.1, 0.1])
# dummy_tf = RigidTransform(dummy_rotation, dummy_translation, from_frame='gripper', to_frame='base')
# dummy_pose_stamped = create_pose_stamped_from_rigid_tf(dummy_tf)
# dummy_pose_stamped.header.frame_id = 'base'
# for i in range(10):
# dummy_plan = planner.plan_to_pose(dummy_pose_stamped)
# if len(dummy_plan.joint_trajectory.points) > 0:
# print 'Found plan!'
# break
# else:
# print 'Unable to find plan. Retrying...'
# import pdb; pdb.set_trace()
# visualize_path(dummy_plan, dummy_translation, planned_path_pub, goal_pos_pub)
# # import pdb; pdb.set_trace()
# raw_input('press enter to go to dummy position \n')
# planner.execute_plan(dummy_plan)
easy_rotation = RigidTransform.rotation_from_quaternion(
np.array([0.012, -0.172, 0.984, -0.025]))
# intermediate position ######################################################
# import pdb; pdb.set_trace()
final_rotation = T_grasp_world.rotation
final_translation = T_grasp_world.translation
z = T_grasp_world.z_axis
easy_z = np.array([0., 0., -1.])
intermediate_trans = final_translation - 0.05 * easy_z
intermediate_tf = RigidTransform(easy_rotation,
intermediate_trans,
from_frame='right_gripper',
to_frame='base')
# import pdb; pdb.set_trace()
intermediate_pose_stamped = create_pose_stamped_from_rigid_tf(
intermediate_tf, 'base')
intermediate_plan = plan_to_pose_custom(intermediate_pose_stamped,
planner)
visualize_path(intermediate_plan, intermediate_trans, planned_path_pub,
goal_pos_pub)
print 'Intermediate goal position: {0}'.format(intermediate_trans)
# import pdb; pdb.set_trace()
raw_input('press enter to go to intermediate position \n')
planner.execute_plan(intermediate_plan)
# final position ######################################################
easy_T_grasp_world = RigidTransform(easy_rotation,
final_translation,
from_frame='right_gripper',
to_frame='base')
final_pose_stamped = create_pose_stamped_from_rigid_tf(
easy_T_grasp_world, 'base')
final_plan = plan_to_pose_custom(final_pose_stamped, planner)
visualize_path(final_plan, T_grasp_world.translation, planned_path_pub,
goal_pos_pub)
print 'Final goal position: {0}'.format(final_translation)
# import pdb; pdb.set_trace()
raw_input('press enter to go to final pose')
planner.execute_plan(final_plan)
# close gripper ######################################################
raw_input('press enter to close the gripper')
close_gripper()
# lift ######################################################
lift_translation = final_translation
lift_translation[2] += 0.1
lift_tf = RigidTransform(easy_rotation,
lift_translation,
from_frame='right_gripper',
to_frame='base')
lift_pose_stamped = create_pose_stamped_from_rigid_tf(lift_tf, 'base')
lift_plan = plan_to_pose_custom(lift_pose_stamped, planner)
visualize_path(lift_plan, lift_translation, planned_path_pub,
goal_pos_pub)
print 'Lift goal position: {0}'.format(lift_translation)
# import pdb; pdb.set_trace()
raw_input('press enter to lift the object')
planner.execute_plan(lift_plan)
# move and lower ######################################################
move_lower_translation = final_translation
move_lower_translation[0] -= 0.1
move_lower_translation[1] -= 0.1
move_lower_translation[2] -= 0.03
move_lower_tf = RigidTransform(easy_rotation,
move_lower_translation,
from_frame='right_gripper',
to_frame='base')
move_lower_pose_stamped = create_pose_stamped_from_rigid_tf(
move_lower_tf, 'base')
move_lower_plan = plan_to_pose_custom(move_lower_pose_stamped, planner)
visualize_path(move_lower_plan, move_lower_translation,
planned_path_pub, goal_pos_pub)
print 'Move and lower goal position: {0}'.format(
move_lower_translation)
# import pdb; pdb.set_trace()
raw_input('press enter to move and lower the object')
planner.execute_plan(move_lower_plan)
# open gripper ######################################################
raw_input('press enter to open the gripper')
open_gripper()
