def get_obj_frames(blue_obj, red_obj, base_tf, base2kinect_tf):
base_pose = Pose()
base_pose.position = base_tf.transform.translation
base_pose.orientation = base_tf.transform.rotation
base_kdl = tf_conversions.fromMsg(base_pose)
base_unitX = base_kdl.M.UnitX()
base_unitY = base_kdl.M.UnitY()
base_unitZ = base_kdl.M.UnitZ()
### Frame for Blue Object
blue_center_kinect = PointStamped()
blue_center_kinect.header = base2kinect_tf.header
blue_center_kinect.point = blue_obj.bb_center
blue_center = tf2_geometry_msgs.do_transform_point(blue_center_kinect,
base2kinect_tf)
blue_pose = Pose()
blue_pose.position = blue_center.point
blue_pose.position.z = blue_pose.position.z - blue_obj.bb_dims.z / 2
blue_pose.orientation = base_tf.transform.rotation
blue_kdl = tf_conversions.fromMsg(blue_pose)
blue_pos = blue_kdl.p
blue_rot = PyKDL.Rotation(-base_unitX, -base_unitY, base_unitZ)
blue_kdl = PyKDL.Frame(blue_rot, blue_pos)
blue_pose = tf_conversions.toMsg(blue_kdl)
blue_frame = TransformStamped()
blue_frame.header.frame_id = base_frame
blue_frame.header.stamp = rospy.Time.now()
blue_frame.child_frame_id = 'blue_frame'
blue_frame.transform.translation = blue_pose.position
blue_frame.transform.rotation = blue_pose.orientation
### Frame for Red Object
red_center_kinect = PointStamped()
red_center_kinect.header = base2kinect_tf.header
red_center_kinect.point = red_obj.bb_center
red_center = tf2_geometry_msgs.do_transform_point(red_center_kinect,
base2kinect_tf)
red_pose = Pose()
red_pose.position = red_center.point
red_pose.position.z = red_pose.position.z - red_obj.bb_dims.z / 2
red_pose.orientation = base_tf.transform.rotation
red_kdl = tf_conversions.fromMsg(red_pose)
red_pos = red_kdl.p
red_rot = PyKDL.Rotation(-base_unitX, -base_unitY, base_unitZ)
red_kdl = PyKDL.Frame(red_rot, red_pos)
red_pose = tf_conversions.toMsg(red_kdl)
red_frame = TransformStamped()
red_frame.header.frame_id = base_frame
red_frame.header.stamp = rospy.Time.now()
red_frame.child_frame_id = 'red_frame'
red_frame.transform.translation = red_pose.position
red_frame.transform.rotation = red_pose.orientation
return blue_frame, red_frame
def publish_goal(self, pose, lookahead_distance=0.0, stop_at_goal=False):
goal = Goal()
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = "odom"
lookahead = tf_conversions.Frame(
tf_conversions.Vector(lookahead_distance, 0, 0))
# add the offset for navigating to the goal:
# (offset * odom).Inverse() * goal = odom.Invserse() * pose
# goal = (offset * odom) * odom.Inverse() * pose
# goal = offset * pose
original_pose_frame = tf_conversions.fromMsg(pose)
pose_frame = self.zero_odom_offset * original_pose_frame
original_pose_frame_lookahead = original_pose_frame * lookahead
pose_frame_lookahead = pose_frame * lookahead
goal.pose.pose.position.x = pose_frame_lookahead.p.x()
goal.pose.pose.position.y = pose_frame_lookahead.p.y()
goal.pose.pose.orientation = tf_conversions.toMsg(
pose_frame_lookahead).orientation
goal.stop_at_goal.data = stop_at_goal
self.goal_pub.publish(goal)
self.goal_plus_lookahead = tf_conversions.toMsg(
original_pose_frame_lookahead)
if self.publish_gt_goals:
try:
trans = self.tfBuffer.lookup_transform('map', 'odom',
rospy.Time())
trans_frame = tf_conversions.Frame(
tf_conversions.Rotation(trans.rotation.x, trans.rotation.y,
trans.rotation.z,
trans.rotation.w),
tf_conversions.Vector(trans.translation.x,
trans.translation.y,
trans.translation.z))
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = "map"
goal_pose.pose = tf_conversions.toMsg(trans_frame *
pose_frame_lookahead)
self.goal_pose_pub.publish(goal_pose)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print('Could not lookup transform from /map to /odom')
pass
def make_marker(m_id, pose, frame_id="base_link", color=(1, 0, 0, 1), scale=(.1, .03, .01), m_type=visualization_msgs.msg.Marker.ARROW, arrow_direction='x'):
final_marker_pose = tf_conversions.fromMsg(pose)
# For a given pose, the marker can point along either the x, y or z axis. By default, ros rvis points along x axis
if arrow_direction == 'x':
pass
elif arrow_direction == 'y':
final_marker_pose.M.DoRotZ(math.pi / 2)
elif arrow_direction == 'z':
final_marker_pose.M.DoRotY(-math.pi / 2)
else:
print "Invalid arrow direction, using default x "
m = visualization_msgs.msg.Marker()
m.id = m_id
m.type = m_type
m.header.frame_id = frame_id
m.header.stamp = rospy.Time.now()
m.pose = tf_conversions.toMsg(final_marker_pose)
m.color = std_msgs.msg.ColorRGBA(*color)
m.scale = Vector3(*scale)
return m
def add_waypoint(self):
if self.new_waypoint_name:
try:
F_waypoint = tf_c.fromTf(
self.listener_.lookupTransform('/world', '/endpoint',
rospy.Time(0)))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logerr(
'Could not find the tf frame for the robot endpoint')
return
try:
rospy.wait_for_service('/instructor_core/AddWaypoint', 2)
except rospy.ROSException as e:
rospy.logerr(e)
return
try:
add_waypoint_proxy = rospy.ServiceProxy(
'/instructor_core/AddWaypoint', AddWaypoint)
msg = AddWaypointRequest()
msg.name = '/' + self.new_waypoint_name
msg.world_pose = tf_c.toMsg(F_waypoint)
rospy.loginfo(add_waypoint_proxy(msg))
self.update_waypoints()
except rospy.ServiceException, e:
rospy.logerr(e)
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time(0), timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(grasp_frame, move_group_commander.get_end_effector_link(),rospy.Time())
except:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s"%(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link':
rospy.logerr('This is a PR2\'s left arm so we have to rotate things.')
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], tf.transformations.quaternion_from_euler(0,math.pi/2,0))
else:
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], [0,0,0])
actual_ee_pose_matrix = np.dot( graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
def remove_anchor_pose(cls, anchor, data):
# Convert anchor pose into a PyKDL.Frame: simplifies
anchor_frame = tf_conversions.fromMsg(anchor.pose)
anchor_frame.M = PyKDL.Rotation() # NO ROTATION
def subtract_pose(point, verbose=False):
p = copy.deepcopy(point)
# Find the difference in poses. NOTE we do not change anything other
# than the pose (twist & wrench stay the same).
point_frame = tf_conversions.fromMsg(point.pose)
delta = anchor_frame.Inverse() * point_frame
p.pose = tf_conversions.toMsg(delta)
if verbose:
print('{} {} {} -> {} {} {}'.format(point.pose.position.x,
point.pose.position.y,
point.pose.position.z,
p.pose.position.x,
p.pose.position.y,
p.pose.position.z))
return p
parsed = [subtract_pose(x) for x in data]
# Create an identity translation/rotation for the new anchor pose.
anchor_new = copy.deepcopy(anchor)
identity = tf_conversions.Frame()
anchor_new.pose = tf_conversions.toMsg(identity)
return (anchor_new, parsed)
def transform_to_tip(group, ee_frame, goal):
"""
Transform the goal from the ee_frame to the tip link which is
resolved from planning group
return (new_goal, tip_frame)
"""
mc = MoveGroupCommander(group)
tip_frame = mc.get_end_effector_link()
ee_pose = mc.get_current_pose(ee_frame)
tip_pose = mc.get_current_pose(tip_frame)
kdl_ee = tf_conversions.fromMsg(ee_pose.pose)
kdl_tip = tf_conversions.fromMsg(tip_pose.pose)
if isinstance(goal, geometry_msgs.msg.Pose):
kdl_goal = tf_conversions.fromMsg(goal)
elif isinstance(goal, geometry_msgs.msg.PoseStamped):
kdl_goal = tf_conversions.fromMsg(goal.pose)
else:
rospy.logerr("Unknown pose type, only Pose and PoseStamped is allowed")
rospy.logerr(str(type(goal)))
return (None, tip_frame)
grip = kdl_tip.Inverse() * kdl_ee
kdl_pose = kdl_goal * grip.Inverse()
return (tf_conversions.toMsg(kdl_pose), tip_frame)
def update(self):
if not self.driver_status == 'DISCONNECTED':
# Get Joint Positions
self.current_joint_positions = self.rob.getj()
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "robot_secondary_interface_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0]*6
msg.effort = [0]*6
self.joint_state_publisher.publish(msg)
# Get TCP Position
tcp_angle_axis = self.rob.getl()
# Create Frame from XYZ and Angle Axis
T = PyKDL.Frame()
axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
# Get norm and normalized axis
angle = axis.Normalize()
# Make frame
T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
T.M = PyKDL.Rotation.Rot(axis,angle)
# Create Pose
self.current_tcp_pose = tf_c.toMsg(T)
self.current_tcp_frame = T
self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time(0), timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(grasp_frame, move_group_commander.get_end_effector_link(),rospy.Time())
except:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s"%(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot( graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
def add_frame_to_proto(self, frame_id, proto_msg):
pose_msg = None
try:
self.listener.waitForTransform(self.graspit_frame, frame_id,
rospy.Time(0), rospy.Duration(100))
pose_msg = tf_conversions.toMsg(
tf_conversions.fromTf(
self.listener.lookupTransform(self.graspit_frame, frame_id,
rospy.Time(0))))
except Exception as e:
rospy.logerr(
self.__class__.__name__ + "::" +
"Failed to lookup frame transform from %s to %s -- %s" %
(self.graspit_frame, frame_id, e.message))
if pose_msg:
frame_proto = proto_msg.renderable.frame
frame_proto.frame_id = frame_id
frame_proto.orientation.w = pose_msg.orientation.w
frame_proto.orientation.x = pose_msg.orientation.x
frame_proto.orientation.y = pose_msg.orientation.y
frame_proto.orientation.z = pose_msg.orientation.z
frame_proto.translation.x = pose_msg.position.x
frame_proto.translation.y = pose_msg.position.y
frame_proto.translation.z = pose_msg.position.z
frame_proto.units = 1.0
proto_msg.renderable.renderableFrame = frame_id.strip('/')
return proto_msg
def servo_fn(self,val,*args):
if self.driver_status != 'SERVO':
rospy.logwarn('ROBOT NOT IN SERVO MODE')
return
else:
rospy.wait_for_service('/simple_ur_msgs/ServoToPose')
try:
pose_servo_proxy = rospy.ServiceProxy('/simple_ur_msgs/ServoToPose',ServoToPose)
F_target_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/target_frame',rospy.Time(0)))
F_target_base = tf_c.fromTf(self.listener_.lookupTransform('/base_link','/target_frame',rospy.Time(0)))
F_base_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/base_link',rospy.Time(0)))
F_command = F_base_world.Inverse()*F_target_world
msg = ServoToPoseRequest()
msg.target = tf_c.toMsg(F_command)
msg.accel = .7
msg.vel = .3
# Send Servo Command
rospy.logwarn('Single Servo Move Started')
result = pose_servo_proxy(msg)
rospy.logwarn('Single Servo Move Finished')
rospy.logwarn(str(result.ack))
except rospy.ServiceException, e:
print e
def mean_arm_pose(self, arm_poses):
tmp = kdl.Vector(0.0, 0.0, 0.0)
elbow_angle = 0.0
direction = kdl.Vector()
for m in arm_poses:
elbow_angle = elbow_angle + m.elbow_angle
p = tfc.fromMsg(m.direction)
tmp = p.M * kdl.Vector(1.0, 0.0, 0.0)
direction = direction + tmp
n = direction.Normalize()
# rospy.loginfo('x: {} y: {} z: {}'.format(direction.x(), direction.y(), direction.z()))
elbow_angle = elbow_angle / len(arm_poses)
pitch = math.atan2(-direction.z(), math.sqrt(direction.x()*direction.x() + direction.y()*direction.y()))
yaw = math.atan2(direction.y(), direction.x())
pose = kdl.Frame(kdl.Rotation.RPY(0.0, pitch, yaw))
arm_msg = copy.deepcopy(arm_poses[-1])
arm_msg.direction = tfc.toMsg(pose)
arm_msg.elbow_angle = elbow_angle
max_dev = 0.0
for m in arm_poses:
p = tfc.fromMsg(m.direction)
tmp = p.M * kdl.Vector(1.0, 0.0, 0.0)
dev = (direction - tmp).Norm()
if dev > max_dev: max_dev = dev
return arm_msg, max_dev
def update(self):
if not self.driver_status == 'DISCONNECTED':
# Get Joint Positions
self.current_joint_positions = self.rob.getj()
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "robot_secondary_interface_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0] * 6
msg.effort = [0] * 6
self.joint_state_publisher.publish(msg)
# Get TCP Position
tcp_angle_axis = self.rob.getl()
# Create Frame from XYZ and Angle Axis
T = PyKDL.Frame()
axis = PyKDL.Vector(tcp_angle_axis[3], tcp_angle_axis[4],
tcp_angle_axis[5])
# Get norm and normalized axis
angle = axis.Normalize()
# Make frame
T.p = PyKDL.Vector(tcp_angle_axis[0], tcp_angle_axis[1],
tcp_angle_axis[2])
T.M = PyKDL.Rotation.Rot(axis, angle)
# Create Pose
self.current_tcp_pose = tf_c.toMsg(T)
self.current_tcp_frame = T
self.broadcaster_.sendTransform(tuple(T.p),
tuple(T.M.GetQuaternion()),
rospy.Time.now(), '/endpoint',
'/base_link')
def transform2pose(transform, frame_id=None, time=None):
msg = PoseStampedMsg()
T = tf_conversions.fromTf(transform)
msg.pose = tf_conversions.toMsg(T)
msg.header.frame_id = frame_id
msg.header.stamp = time
return msg.pose if time is None else msg
def matrix2pose(matrix, frame_id=None, time=None):
msg = PoseStampedMsg()
T = tf_conversions.fromMatrix(matrix)
msg.pose = tf_conversions.toMsg(T)
msg.header.frame_id = frame_id
msg.header.stamp = time
return msg.pose if time is None else msg
def item_frame_to_pose(item_frame, frame_id):
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = frame_id
goal_pose.pose = toMsg(item_frame)
return goal_pose
def add_frame_to_proto(self, frame_id, proto_msg):
pose_msg = None
try:
self.listener.waitForTransform(self.graspit_frame, frame_id, rospy.Time(0), rospy.Duration(100))
pose_msg = tf_conversions.toMsg(tf_conversions.fromTf(self.listener.lookupTransform(self.graspit_frame,
frame_id,
rospy.Time(0))))
except Exception as e:
rospy.logerr(self.__class__.__name__ + "::" +
"Failed to lookup frame transform from %s to %s -- %s"%(self.graspit_frame,
frame_id, e.message))
if pose_msg:
frame_proto = proto_msg.renderable.frame
frame_proto.frame_id = frame_id
frame_proto.orientation.w = pose_msg.orientation.w
frame_proto.orientation.x = pose_msg.orientation.x
frame_proto.orientation.y = pose_msg.orientation.y
frame_proto.orientation.z = pose_msg.orientation.z
frame_proto.translation.x = pose_msg.position.x
frame_proto.translation.y = pose_msg.position.y
frame_proto.translation.z = pose_msg.position.z
frame_proto.units = 1.0
proto_msg.renderable.renderableFrame = frame_id.strip('/')
return proto_msg
def axis2pose(rvec, tvec, frame_id=None, time=None):
msg = PoseStampedMsg()
T = axis2matrix(rvec, tvec)
T = tf_conversions.fromMatrix(T)
msg.pose = tf_conversions.toMsg(T)
msg.header.frame_id = frame_id
msg.header.stamp = time
return msg.pose if time is None else msg
def ray_to_location(self, ray_dir_frame, **kwargs):
pointer_frame = self.intersect(ray_dir_frame, **kwargs)
pointer_pose = None
if pointer_frame:
pointer_pose = tfc.toMsg(pointer_frame)
return pointer_pose
def orient_towards_object_double(tfBuffer, ee_vec, object_pose):
w2w = tfBuffer.lookup_transform(base_frame, base_frame, rospy.Time(0))
w2wPose = Pose()
w2wPose.position.x = w2w.transform.translation.x
w2wPose.position.y = w2w.transform.translation.y
w2wPose.position.z = w2w.transform.translation.z
w2wPose.orientation = w2w.transform.rotation
ee_pose = Pose()
ee_pose.position.x = ee_vec[0]
ee_pose.position.y = ee_vec[1]
ee_pose.position.z = ee_vec[2]
ee_pose.orientation.x = ee_vec[3]
ee_pose.orientation.y = ee_vec[4]
ee_pose.orientation.z = ee_vec[5]
ee_pose.orientation.w = ee_vec[6]
w2i_kdl = tf_conversions.fromMsg(ee_pose)
w2i_x = w2i_kdl.M.UnitX()
w2i_pos = w2i_kdl.p
w2e_kdl = tf_conversions.fromMsg(object_pose)
w2e_x = w2e_kdl.M.UnitX()
w2e_pos = w2e_kdl.p
w2w_kdl = tf_conversions.fromMsg(w2wPose)
w2w_z = w2w_kdl.M.UnitZ()
w2w_pos = w2w_kdl.p
z_rot_wp = (w2e_pos - w2i_pos) / ((w2e_pos - w2i_pos).Norm())
y_rot_wp = -w2w_z * z_rot_wp
x_rot_wp = -z_rot_wp * y_rot_wp
wp_pos = w2i_pos
wp_M = PyKDL.Rotation(x_rot_wp, y_rot_wp, z_rot_wp)
wp_kdl = PyKDL.Frame(wp_M, wp_pos)
wp_pose = tf_conversions.toMsg(wp_kdl)
recipNorm = 1 / math.sqrt(wp_pose.orientation.x**2 +
wp_pose.orientation.y**2 +
wp_pose.orientation.z**2 +
wp_pose.orientation.w**2)
wp_pose.orientation.x = wp_pose.orientation.x * recipNorm
wp_pose.orientation.y = wp_pose.orientation.y * recipNorm
wp_pose.orientation.z = wp_pose.orientation.z * recipNorm
wp_pose.orientation.w = wp_pose.orientation.w * recipNorm
wp_orientation = []
wp_orientation.append(wp_pose.orientation.x)
wp_orientation.append(wp_pose.orientation.y)
wp_orientation.append(wp_pose.orientation.z)
wp_orientation.append(wp_pose.orientation.w)
return wp_orientation
def run(self):
loop_rate = rospy.Rate(self.publish_rate)
while not rospy.is_shutdown():
try:
loop_rate.sleep()
if self.ws_shape:
if self.ws_shape != self.last_ws_shape:
self.pub_workspace_shape.publish(self.get_shape_name(self.ws_shape))
self.last_ws_shape = self.ws_shape
ray_tf = self.pointing_model.pointing_ray()
if ray_tf:
ray_kdl_frame = transform_to_kdl(ray_tf)
self.tf_br.sendTransform(ray_tf)
ray_pose = PoseStamped()
ray_pose.header = ray_tf.header
ray_pose.pose = tfc.toMsg(ray_kdl_frame)
self.pub_pointing_ray.publish(ray_pose)
ray_origin_kdl_frame = self.frame_from_tf(self.human_frame, self.ray_origin_frame)
shape_origin_kdl_frame = self.frame_from_tf(self.human_frame, self.ws_shape.frame_id)
if ray_origin_kdl_frame and shape_origin_kdl_frame:
pointer_pose = self.get_pointer(self.ws_shape, ray_kdl_frame)
if pointer_pose:
m_msg = self.create_rviz_marker(self.ws_shape, self.ws_shape.point - copy.deepcopy(shape_origin_kdl_frame.p))
if m_msg:
self.pub_markers.publish(m_msg)
pose_msg = PoseStamped()
pose_msg.header = ray_tf.header
pose_msg.pose = pointer_pose
self.pub_arm_pointer.publish(pose_msg)
eyes_pointer = tfc.fromMsg(pointer_pose).p - ray_origin_kdl_frame.p
pr_marker_msg = self.create_pointing_ray_marker(self.pointing_model.frame_id, eyes_pointer.Norm())
if pr_marker_msg:
self.pub_markers.publish(pr_marker_msg)
except rospy.ROSException, e:
if e.message == 'ROS time moved backwards':
rospy.logwarn('Saw a negative time change, resetting.')
def save_data_at_goal(self, pose, goal_odom, goal_world, theta_offset,
path_offset):
if self.save_gt_data:
try:
trans = self.tfBuffer.lookup_transform('map', 'base_link',
rospy.Time())
trans_as_text = json.dumps(
message_converter.convert_ros_message_to_dictionary(trans))
with open(
self.save_dir +
('%06d_map_to_base_link.txt' % self.goal_number),
'w') as pose_file:
pose_file.write(trans_as_text)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print('Could not lookup transform from /map to /base_link')
pass
# save full res image at goal
if self.save_full_res_image_at_goal:
cv2.imwrite(
self.save_dir + ('full/goal_%06d.png' % self.goal_number),
self.last_full_res_image)
# save current pose info
message_as_text = json.dumps(
message_converter.convert_ros_message_to_dictionary(pose))
with open(self.save_dir + ('pose/%06d_pose.txt' % self.goal_number),
'w') as pose_file:
pose_file.write(message_as_text)
# save current offset
offset = tf_conversions.toMsg(goal_odom.Inverse() * goal_world)
message_as_text = json.dumps(
message_converter.convert_ros_message_to_dictionary(offset))
with open(
self.save_dir + ('offset/%06d_offset.txt' % self.goal_number),
'w') as offset_file:
offset_file.write(message_as_text)
# publish offset to tf
# self.tf_pub.sendTransform((offset.position.x, offset.position.y, offset.position.z), (offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w), rospy.Time.now(), 'map', 'odom')
# publish current corrections
message_as_text = json.dumps({
'theta_offset': theta_offset,
'path_offset': path_offset
})
with open(
self.save_dir +
('correction/%06d_correction.txt' % self.goal_number),
'w') as correction_file:
correction_file.write(message_as_text)
def update_goal(self, goal_frame, new_goal=True, turning_goal=False):
if new_goal:
self.last_goal = self.goal
self.goal = tf_conversions.toMsg(goal_frame)
normalise_quaternion(self.goal.orientation)
# if goal is a turning goal, or the last goal - don't set virtual waypoint ahead
if turning_goal or (self.goal_index == len(self.poses) - 1
and self.stop_at_end):
self.publish_goal(self.goal, 0.0, True)
else:
self.publish_goal(
self.goal, (LOOKAHEAD_DISTANCE_RATIO * GOAL_DISTANCE_SPACING),
False)
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander,
listener,
graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame,
move_group_commander.get_end_effector_link(),
rospy.Time(0),
timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time())
except:
rospy.logerr(
"graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s" %
(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS(
).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link':
rospy.logerr('This is a PR2\'s left arm so we have to rotate things.')
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation(
[0, 0, 0],
tf.transformations.quaternion_from_euler(0, math.pi / 2, 0))
else:
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation(
[0, 0, 0], [0, 0, 0])
actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix,
approach_tran_to_end_effector_tran_matrix)
actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat)
actual_ee_pose = tf_conversions.toMsg(
tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
def get_graspit_pose_to_moveit_pose(moveit_grasp_msg, graspit_tran):
listener = tf.listener.TransformListener()
listener.waitForTransform("approach_tran", 'staubli_rx60l_link7', rospy.Time(), timeout=rospy.Duration(1.0))
print '3'
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform("approach_tran", 'staubli_rx60l_link7',rospy.Time())
print '4'
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot( graspit_tran, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
moveit_grasp_msg_copy = deepcopy(moveit_grasp_msg)
moveit_grasp_msg_copy.grasp_pose.pose = actual_ee_pose
return moveit_grasp_msg_copy
def send_command(self):
self.current_joint_positions = self.q
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "simuated_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0]*6
msg.effort = [0]*6
self.joint_state_publisher.publish(msg)
pose = self.kdl_kin.forward(self.q)
F = tf_c.fromMatrix(pose)
# F = self.F_command
self.current_tcp_pose = tf_c.toMsg(F)
self.current_tcp_frame = F
self.broadcaster_.sendTransform(tuple(F.p),tuple(F.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def subtract_pose(point, verbose=False):
p = copy.deepcopy(point)
# Find the difference in poses. NOTE we do not change anything other
# than the pose (twist & wrench stay the same).
point_frame = tf_conversions.fromMsg(point.pose)
delta = anchor_frame.Inverse() * point_frame
p.pose = tf_conversions.toMsg(delta)
if verbose:
print('{} {} {} -> {} {} {}'.format(point.pose.position.x,
point.pose.position.y,
point.pose.position.z,
p.pose.position.x,
p.pose.position.y,
p.pose.position.z))
return p
def make_service_call(self, request, *args):
# Check to see if service exists
try:
rospy.wait_for_service('costar/ServoToPose')
except rospy.ROSException as e:
rospy.logerr('Could not find servo service')
self.finished_with_success = False
return
# Make servo call to set pose
try:
pose_servo_proxy = rospy.ServiceProxy('costar/ServoToPose',
ServoToPose)
F_command_world = tf_c.fromTf(
self.listener_.lookupTransform(
'/world', '/' + self.command_waypoint_name, rospy.Time(0)))
F_base_world = tf_c.fromTf(
self.listener_.lookupTransform('/world', '/base_link',
rospy.Time(0)))
F_command = F_base_world.Inverse() * F_command_world
msg = costar_robot_msgs.srv.ServoToPoseRequest()
msg.target = tf_c.toMsg(F_command)
msg.vel = self.command_vel
msg.accel = self.command_acc
# Send Servo Command
rospy.loginfo('Single Servo Move Started')
result = pose_servo_proxy(msg)
if 'FAILURE' in str(result.ack):
rospy.logwarn('Servo failed with reply: ' + str(result.ack))
self.finished_with_success = False
return
else:
rospy.loginfo('Single Servo Move Finished')
rospy.loginfo('Robot driver reported: ' + str(result.ack))
self.finished_with_success = True
return
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, rospy.ServiceException), e:
rospy.logerr('There was a problem with the tf lookup or service:')
rospy.logerr(e)
self.finished_with_success = False
return
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def transform_pose(grasp_pose_in_world, world_frame_to_object_frame_transform):
# Convert ROSmsg to 4x4 numpy arrays
grasp_pose_in_world_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(grasp_pose_in_world))
object_pose_in_world_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(world_frame_to_object_frame_transform))
# Transform the grasp pose into the object reference frame
grasp_pose_in_object_tran_matrix = np.dot(
np.linalg.inv(object_pose_in_world_tran_matrix),
grasp_pose_in_world_tran_matrix)
# Convert pose back into ROS message
grasp_pose_in_object = tf_conversions.toMsg(
tf_conversions.fromMatrix(grasp_pose_in_object_tran_matrix))
return grasp_pose_in_object
def graspit_grasp_pose_to_moveit_grasp_pose(
listener, # type: tf.TransformListener
graspit_grasp_msg, # type: graspit_msgs.msg.Grasp
end_effector_link, # type: str
grasp_frame # type: str
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param listener: A transformer for looking up the transformation
:param graspit_grasp_msg: A graspit grasp message
"""
try:
listener.waitForTransform(
grasp_frame,
end_effector_link,
rospy.Time(0),
timeout=rospy.Duration(1)
)
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
grasp_frame,
end_effector_link,
rospy.Time()
)
except Exception as e:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n "
"Failed to find transform from {} to {}"
.format(grasp_frame, end_effector_link)
)
ipdb.set_trace()
return geometry_msgs.msg.Pose()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose)
)
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
return actual_ee_pose
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def update(self):
if not self.follow == 'DISABLED':
try:
if self.follow == 'MARKER':
F_target_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/target_frame',rospy.Time(0)))
F_target_base = tf_c.fromTf(self.listener_.lookupTransform('/base_link','/target_frame',rospy.Time(0)))
elif self.follow == 'INTERACT':
F_target_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/endpoint_interact',rospy.Time(0)))
F_target_base = tf_c.fromTf(self.listener_.lookupTransform('/base_link','/endpoint_interact',rospy.Time(0)))
F_base_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/base_link',rospy.Time(0)))
F_command = F_base_world.Inverse()*F_target_world
cmd = PoseStamped()
cmd.pose = tf_c.toMsg(F_command)
self.target_pub.publish(cmd)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn(str(e))
def create_correction(self, word, robot_anchor):
""" Create an AnchoredCorrection message using the given word & anchor.
Get the correction points from the stored map, using a *single* word.
"""
if word not in self._corrections:
rospy.logerr('Word [{}] not in known corrections: {}'.format(
word, self._corrections.keys()))
return None
# Make sure we have a string, not a list of strings.
assert isinstance(word, basestring)
correction = AnchoredDemonstration()
correction.header.stamp = rospy.Time.now()
correction.words.append(word)
correction.num_words = 1
# Set the corrections: Transform each demonstrated point so it begins
# from the robot anchor.
correction.num_points = len(self._corrections[word])
anchor_frame = tf_conversions.fromMsg(
robot_anchor.pose) # PyKDL.Frame.
anchor_frame.M = PyKDL.Rotation() # No rotation.
for c in self._corrections[word]:
# Convert each pose to be in the world frame, using the anchor.
pose_frame = tf_conversions.fromMsg(c.pose)
offset = anchor_frame * pose_frame
# Copy the correction and update the *pose only*.
new_c = copy.deepcopy(c)
new_c.pose = tf_conversions.toMsg(offset)
correction.demonstration.append(new_c)
pass
# Keep the anchor pose to indicate where the correction starts from.
correction.anchor = robot_anchor
return correction
def add_waypoint(self):
if self.new_waypoint_name:
try:
F_waypoint = tf_c.fromTf(self.listener_.lookupTransform('/world','/endpoint',rospy.Time(0)))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logerr('Could not find the tf frame for the robot endpoint')
return
try:
rospy.wait_for_service('/instructor_core/AddWaypoint',2)
except rospy.ROSException as e:
rospy.logerr(e)
return
try:
add_waypoint_proxy = rospy.ServiceProxy('/instructor_core/AddWaypoint',AddWaypoint)
msg = AddWaypointRequest()
msg.name = '/' + self.new_waypoint_name
msg.world_pose = tf_c.toMsg(F_waypoint)
rospy.loginfo(add_waypoint_proxy(msg))
self.update_waypoints()
except rospy.ServiceException, e:
rospy.logerr(e)
def graspit_grasp_pose_to_moveit_grasp_pose(
listener, # type: tf.TransformListener
graspit_grasp_msg, # type: graspit_interface.msg.Grasp
end_effector_link, # type: str
grasp_frame # type: str
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param listener: A transformer for looking up the transformation
:param graspit_grasp_msg: A graspit grasp message
"""
try:
listener.waitForTransform(grasp_frame,
end_effector_link,
rospy.Time(0),
timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
grasp_frame, end_effector_link, rospy.Time())
except Exception as e:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n "
"Failed to find transform from {} to {}".format(
grasp_frame, end_effector_link))
ipdb.set_trace()
return geometry_msgs.msg.Pose()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg.pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS(
).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix,
approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(
tf_conversions.fromMatrix(actual_ee_pose_matrix))
return actual_ee_pose
def make_service_call(self,request,*args):
# Check to see if service exists
try:
rospy.wait_for_service('/simple_ur_msgs/ServoToPose')
except rospy.ROSException as e:
rospy.logerr('Could not find servo service')
self.finished_with_success = False
return
# Make servo call to set pose
try:
pose_servo_proxy = rospy.ServiceProxy('/simple_ur_msgs/ServoToPose',ServoToPose)
F_command_world = tf_c.fromTf(self.listener_.lookupTransform('/world', '/'+self.command_waypoint_name, rospy.Time(0)))
F_base_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/base_link',rospy.Time(0)))
F_command = F_base_world.Inverse()*F_command_world
msg = simple_ur_msgs.srv.ServoToPoseRequest()
msg.target = tf_c.toMsg(F_command)
msg.vel = self.command_vel
msg.accel = self.command_acc
# Send Servo Command
rospy.loginfo('Single Servo Move Started')
result = pose_servo_proxy(msg)
if 'FAILURE' in str(result.ack):
rospy.logwarn('Servo failed with reply: '+ str(result.ack))
self.finished_with_success = False
return
else:
rospy.loginfo('Single Servo Move Finished')
rospy.logwarn('Robot driver reported: '+str(result.ack))
self.finished_with_success = True
return
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, rospy.ServiceException), e:
rospy.logwarn('There was a problem with the tf lookup or service:')
rospy.logerr(e)
self.finished_with_success = False
return
def get_graspit_pose_to_moveit_pose(moveit_grasp_msg, graspit_tran):
listener = tf.listener.TransformListener()
listener.waitForTransform("approach_tran",
'staubli_rx60l_link7',
rospy.Time(),
timeout=rospy.Duration(1.0))
print '3'
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
"approach_tran", 'staubli_rx60l_link7', rospy.Time())
print '4'
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS(
).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot(graspit_tran,
approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(
tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
moveit_grasp_msg_copy = deepcopy(moveit_grasp_msg)
moveit_grasp_msg_copy.grasp_pose.pose = actual_ee_pose
return moveit_grasp_msg_copy
def get_graspit_grasp_pose_in_new_reference_frame(
graspit_grasp_msg_pose, # type: geometry_msgs.msg.Pose
target_to_source_translation_rotation, # type: tuple of tuples
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param target_to_source_translation_rotation: result of listener.lookupTransform((target_frame, grasp_frame, rospy.Time(0), timeout=rospy.Duration(1))
:param graspit_grasp_msg_pose: The pose of a graspit grasp message i.e. g.pose
t_T_G = t_T_s * s_T_G
"""
graspit_grasp_pose_in_source_frame_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg_pose)
)
source_in_target_frame_tran_matrix = tf.TransformerROS().fromTranslationRotation(
target_to_source_translation_rotation[0],
target_to_source_translation_rotation[1])
graspit_grasp_pose_in_target_frame_matrix = np.dot(source_in_target_frame_tran_matrix,
graspit_grasp_pose_in_source_frame_matrix) # t_T_G = t_T_s * s_T_G
graspit_grasp_pose_in_target_frame = tf_conversions.toMsg(
tf_conversions.fromMatrix(graspit_grasp_pose_in_target_frame_matrix))
return graspit_grasp_pose_in_target_frame
def change_end_effector_link(
graspit_grasp_msg_pose, # type: geometry_msgs.msg.Pose
old_link_to_new_link_translation_rotation, # type: tuple of tuples
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param old_link_to_new_link_translation_rotation: result of listener.lookupTransform((old_link, new_link, rospy.Time(0), timeout=rospy.Duration(1))
:param graspit_grasp_msg_pose: The pose of a graspit grasp message i.e. g.pose
ref_T_nl = ref_T_ol * ol_T_nl
"""
graspit_grasp_pose_for_old_link_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg_pose)
)
old_link_to_new_link_tranform_matrix = tf.TransformerROS().fromTranslationRotation(
old_link_to_new_link_translation_rotation[0],
old_link_to_new_link_translation_rotation[1])
graspit_grasp_pose_for_new_link_matrix = np.dot(graspit_grasp_pose_for_old_link_matrix,
old_link_to_new_link_tranform_matrix) # ref_T_nl = ref_T_ol * ol_T_nl
graspit_grasp_pose_for_new_link = tf_conversions.toMsg(
tf_conversions.fromMatrix(graspit_grasp_pose_for_new_link_matrix))
return graspit_grasp_pose_for_new_link
def handle_pose_check(self, msg):
#TODO: fix the msg input and function calls used in this node
#self.MoveTriad(msg)
#self.env.RemoveKinBody(self.object)
#rospy.loginfo("handle_pose function called")
#rospy.logdebug(msg)
myrot = PyKDL.Rotation.Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)
r,p,y = PyKDL.Rotation.GetRPY(myrot)
myrot = PyKDL.Rotation.RPY(r, -p, -y)
MyFrame = PyKDL.Frame( myrot , PyKDL.Vector(msg.position.x, msg.position.y, msg.position.z) )
FrameMsg1 = tf_conversions.toMsg(MyFrame)
MyMatrix = tf_conversions.toMatrix(MyFrame)
# Current location of RAVE model hand
tmp = self.manip.GetTransform()
#rospy.logdebug("\n\t\t====Current Matrix====\n" + str(tmp))
# Convert the message to a matrix, MyMatrix will be used to find an IK solution
f1 = tf_conversions.fromMsg(msg)
MyMatrix = tf_conversions.toMatrix(f1)
# rospy.logdebug("\n\t\t====Matrix For Soln====\n" + str(MyMatrix))
# Spread values unused while working with trigrip
#if msg.spread or msg.spread == 0:
# self.robot.SetDOFValues([msg.spread],[10])
with self.env: # Check to see if hand position is in collision. If so, reset hand position.
oldJoints = self.robot.GetDOFValues()
#rospy.logdebug("\n\t====Old joints====\n" + str(oldJoints))
try:
IKtype = IkParameterizationType.Transform6D
#rospy.logdebug(IkParameterization(MyMatrix, IKtype))
# Finds all solutions for a given point and orientation in space
solns = self.manip.FindIKSolutions(IkParameterization(MyMatrix, IKtype), filteroptions=IkFilterOptions.CheckEnvCollisions)
#rospy.loginfo("\n\t===solns===\n" + str(solns))
# Holder for a better solution
soln = None
if solns != []:
# Try to find the solution with the least change in joints 1 and 2, tries to minimize jumping
soln = self.find_soln(solns, oldJoints)
# Could be used in lieu of FindIKSolutions, finds just one solution, not necessarily the best
#soln = self.manip.FindIKSolution(IkParameterization(MyMatrix, IKtype), filteroptions=IkFilterOptions.CheckEnvCollisions)
# We have a solution to apply to the robot
if(soln != None):
#rospy.loginfo("\n\t===soln===\n" + str(soln))
self.robot.SetDOFValues(soln, self.manip.GetArmIndices())
#Send Command to the physical robot
if(msg.move):
# Displays solutions used when A button is pressed
rospy.loginfo("\n\t===Joint Solution===\n" + str(soln))
self.send_cmd(soln)
else:
rospy.loginfo("No IK solution found")
#self.env.AddKinBody(self.object)
return osu_ros_adept.srv.PoseCheckResponse(False,[0,0,0])
except e:
rospy.loginfo("IK not available")
#rospy.logdebug(e)
report=CollisionReport()
for link in self.robot.GetLinks():
linkCollision = self.env.CheckCollision(link,report)
if linkCollision:
rospy.logdebug("Link collision " + str(linkCollision) + "Link: " + str(link))
# Wam code, not used with Adept/Trigrip
if "wam0" in str(link): # If looking at the base of the WAM, skip and continue with next link
continue
incollision=self.env.CheckCollision(link,report)
if incollision:# and msg.j_position[1] == 0 and msg.j_position[2] == 0 or incollision and msg.j_position[3] != 0:
rospy.logdebug("Incollision")
# Detects collision, reset joints
self.robot.SetDOFValues(oldJoints)
rospy.loginfo("Collision detected")
#self.env.AddKinBody(self.object)
return PoseCheckResponse(False,[0,0,0])
#self.MoveTriad(msg)
#if not msg.move: # if not moving, reset robot to old Joint values
# self.robot.SetDOFValues(oldJoints)
if msg.get_norm: # Validated: Return norm is useful when using the Interactive markers to control the WAM
vals = MyMatrix[0:3,2]
#self.env.AddKinBody(self.object)
return PoseCheckResponse(True, vals)
#self.env.AddKinBody(self.object)
return PoseCheckResponse(True,[0,0,0])
y_rot_wp1 = w2w_z * x_rot_wp1
z_rot_wp1 = x_rot_wp1 * y_rot_wp1
wp1_pos = (w2i_pos + w2e1_pos) / 2
x_rot_wp2 = (w2e2_pos - w2i_pos) / ((w2e2_pos - w2i_pos).Norm())
y_rot_wp2 = w2w_z * x_rot_wp2
z_rot_wp2 = x_rot_wp2 * y_rot_wp2
wp2_pos = (w2i_pos + w2e2_pos) / 2
wp1_M = PyKDL.Rotation(x_rot_wp1, y_rot_wp1, z_rot_wp1)
wp2_M = PyKDL.Rotation(x_rot_wp2, y_rot_wp2, z_rot_wp2)
wp1_kdl = PyKDL.Frame(wp1_M, wp1_pos)
wp2_kdl = PyKDL.Frame(wp2_M, wp2_pos)
wp1_pose = tf_conversions.toMsg(wp1_kdl)
wp2_pose = tf_conversions.toMsg(wp2_kdl)
w2mid1 = TransformStamped()
w2mid1.header.frame_id = base_frame
w2mid1.header.stamp = rospy.Time.now()
w2mid1.child_frame_id = 'wp1_frame'
w2mid1.transform.translation.x = wp1_pose.position.x
w2mid1.transform.translation.y = wp1_pose.position.y
w2mid1.transform.translation.z = wp1_pose.position.z
recipNorm1 = 1 / math.sqrt(wp1_pose.orientation.x**2 +
wp1_pose.orientation.y**2 +
wp1_pose.orientation.z**2 +
wp1_pose.orientation.w**2)
w2mid1.transform.rotation.x = wp1_pose.orientation.x * recipNorm1
def UpdateRobotJoints(self, msg):
msg2 = None
#rospy.loginfo('command received')
MyTrans = self.manip.GetTransform() # Get the hand transform
#ftest = tf_conversions.fromMsg(MyTrans)
#ftest = tf_conversions.toMatrix(ftest)
f1 = tf_conversions.fromMsg(msg)
MyMatrix = tf_conversions.toMatrix(f1)
# Create message to set robot commands
move_msg = PoseCheck()
# Copy Postition/Orientation from RobotPose msg to PoseCheck srv
move_msg.position = msg.position
move_msg.orientation = msg.orientation
move_msg.spread = 0.0
# Check if A button was pressed, if so initiate robot movement
if(msg.j_position[0]):
move_msg.move = True
else:
move_msg.move = False
# not currently used so set to false by default
move_msg.get_norm = False
# PRE OSU_ROS_ADEPT CODE, NOT USED IN ADEPT/TRIGRIP PROGRAM
# Function to run if using Interactive Markers
if msg.j_position[3] != 0:
msg2 = PoseCheck()
mycog = self.object.GetTransform()
#Add offset of object and subtract a small amount to fix minor Z offset error.
MyMatrix[0:3,3] = MyMatrix[0:3,3] + mycog[0:3,3] - MyMatrix[0:3,2]*.0175
f2 = tf_conversions.fromMatrix(MyMatrix) # Convert the transform to a ROS frame
msg2.pose = tf_conversions.toMsg(f2)
msg2.move = True
#rospy.logdebug("before receiving error")
if msg.j_position[1] == 0 and msg.j_position[2] == 1:
grasp_msg=GraspObject()
grasp_msg.grasp_direction="open"
grasp_msg.spread=False
grasp_msg.get_joint_vals=False
self.Grasp_Object(grasp_msg) # Open the hand
#self.Grasp_Object(grasp_direction="open") # Open the hand
# END NON OSU_ROS_ADEPT CODE
if msg2 is not None: # Move arm to new position
self.handle_pose_check(msg2)
else:
# Sends osu_ros_adept message
self.handle_pose_check(move_msg)
#TODO: Fix this or remove it for the WAM operation. Cannot use robot.SetTransform.
# For Palm grasp option
if msg.j_position[3] == 2:
HandDirection = MyMatrix[0:3,2]
for i in range(0,1000): # loop until the object is touched or limit is reached
MyMatrix[0:3,3] += .0001 * HandDirection
with env:
self.robot.SetTransform(MyMatrix) # Move hand forward
for link in self.robot.GetLinks(): # Check for collisions
incollision=self.env.CheckCollision(link,report)
if incollision:
i = 1000
if i == 1000: # If hand is in collision, break from the loop
break
# Check to see if any of the manipulator buttons have been pressed. Otherwise, move the hand.
if msg.j_position[1] == 1 and msg.j_position[2] == 0:
grasp_msg=GraspObject()
grasp_msg.grasp_direction="close"
grasp_msg.spread=False
grasp_msg.get_joint_vals=False
self.Grasp_Object(grasp_msg) # Open the hand
#self.Grasp_Object(grasp_direction="close") # Open the hand
return
def set_tran_as_pose(moveit_grasp_msg, tran):
pose_msg = tf_conversions.toMsg(tf_conversions.fromMatrix(tran))
moveit_grasp_msg_copy = deepcopy(moveit_grasp_msg)
moveit_grasp_msg_copy.grasp_pose.pose = pose_msg
return moveit_grasp_msg_copy
def joints_callback(self, pos):
#rospy.logdebug(pos)
# perform calculations to find new location
newpos = tf_conversions.fromMsg(pos)
newpos = tf_conversions.toMatrix(newpos)
#rospy.logdebug(newpos)
# Create two matrices to modify the RPY matrix. This is due to the fact that
# the BarrettWAM hand is rotated 90deg with respect to the global frame.
# The M.GetRPY function and Rotation.RPY use the X-axis as the roll axis
# whereas the WAM wants to roll about the Z-axis. Must rotate the RPY values
# 90deg, then calculate the new RPY coordinates, then rotate back to OpenRAVE
# coordinates again.
#modMatrix = PyKDL.Rotation.RPY(0, -3.14159/2, 0)
#modMatrix2 = PyKDL.Rotation.RPY(0, 3.14159/2, 0)
oldrot = PyKDL.Rotation(newpos[0,0],newpos[0,1],newpos[0,2],newpos[1,0],newpos[1,1],newpos[1,2],newpos[2,0],newpos[2,1],newpos[2,2])
#oldrot = oldrot * modMatrix # rotating matrix
# Get the RPY values from the new rotated matrix
ftest = PyKDL.Frame(oldrot, PyKDL.Vector(0,0,0))
testrot = ftest.M.GetRPY()
# Calculate the new positions and roll, pitch, yaw
roll = testrot[0] + .025 * (self.buttons[RB_IDX] - self.buttons[LB_IDX])
pitch = testrot[1]
if testrot[1] < -1.5 and self.axes[RIGHT_VERT] > 0 or testrot[1] > 1.5 and self.axes[RIGHT_VERT] < 0:
pitch = testrot[1]
else:
pitch = testrot[1] - .025 * self.axes[RIGHT_VERT]
yaw = testrot[2] + .025 * self.axes[RIGHT_HOR]
z = ((self.axes[LEFT_TRIG]-3) - (self.axes[RIGHT_TRIG]-3))
#
#
# Two different styles of control that move the hand in different ways
#
#
# Old move controls
#newpos[0,3] = newpos[0,3] + .01 * self.axes[LEFT_VERT]
#newpos[1,3] = newpos[1,3] + .01 * self.axes[LEFT_HOR]
#newpos[2,3] = newpos[2,3] + .01 * z
#Weight to influence the speed of simulated robot motion
weight = .01
# "Cockpit" style control method
newpos[0,3] = newpos[0,3] - weight * self.axes[LEFT_HOR] * math.sin(yaw) + weight * self.axes[1] * math.cos(yaw) * math.cos(pitch) + weight * z * math.sin(pitch) * math.cos(yaw)
newpos[1,3] = newpos[1,3] + weight * self.axes[LEFT_VERT] * math.sin(yaw) * math.cos(pitch) + weight * self.axes[LEFT_HOR] * math.cos(yaw) + weight * z * math.sin(pitch) * math.sin(yaw)
newpos[2,3] = newpos[2,3] + weight * z * math.cos(pitch) - weight * self.axes[LEFT_VERT] * math.sin(pitch)
#rospy.logdebug('cos, sin, yaw: {0} ,{1} ,{2} ' .format(math.cos(yaw), math.sin(yaw), yaw) )
# Create a frame for publishing. Make sure to rotate back before creating the frame.
v = PyKDL.Vector(newpos[0,3],newpos[1,3],newpos[2,3])
r1 = PyKDL.Rotation.RPY(roll, pitch, yaw)
#r1 = r1 * modMatrix2
f1 = PyKDL.Frame(r1, v)
#the hand is non-existent. left over code from the wam. May be useful if someone attached a hand
# Get the hand values and calculate the new positions
#spread = pos.j_position[0]
movement = self.buttons[A_IDX]
"""if self.buttons[2] and spread < 3.1:
spread = spread + .01
if self.buttons[0] and spread > 0:
spread = spread - .01
"""
# publish the new position
FrameMsg = tf_conversions.toMsg(f1)
j_pos = array([movement,self.buttons[B_IDX],self.buttons[Y_IDX],0])
#rospy.logdebug(self.buttons)
j_vel = array([0,0,0,0])
MyMsg = RobotPose()
MyMsg.position = FrameMsg.position
MyMsg.orientation = FrameMsg.orientation
MyMsg.j_position = j_pos
MyMsg.j_velocity = j_vel
self.jointpub.publish(MyMsg)
heuristicsfile = open(testpath,'w')
heuristicsfile.write('PointArng, TriangleSize, Extension, Spread, Limit, PerpSym, ParallelSym, OrthoNorm, Volume, Hand_Qx, Hand_Qy, Hand_Qz,Hand_Qw,Hand_x,Hand_y,Hand_z, ObjectName, Object_Qx, Object_Qy, Object_Qz, Object_Qw, Object_x, Object_y, Object_Qz, Start_Position,' + str(rospy.Time.now()) + '\n')
heuristicsfile.close()
"""
# Define the OpenRAVE variables necessary for controlling the robot. See OpenRAVE documentation
# for more information
# ****************** Repeat this loop while ROS is running. *************************
while not rospy.is_shutdown():
# Get hand position and robot joints to create new message
MyTrans = MyRave.manip.GetTransform() # Get the hand transform
f2 = tf_conversions.fromMatrix(MyTrans) # Convert the transform to a ROS frame
f2 = tf_conversions.toMsg(f2) # Convert the frame to a message format for publishing
joints = MyRave.robot.GetDOFValues()
#rospy.loginfo("Joints\n" + str(joints))
#rospy.logdebug(MyTrans)
# Use the values retrieved to create new message of current robot location
f = RobotPose()
f.position = f2.position
f.orientation = f2.orientation
#f.j_position = [joints[0],joints[1],joints[2],joints[3]] # For use when only Barrett hand is loaded
#f.j_position = [joints[7],joints[8],joints[9],joints[10]] # Hand joints 1-3(7-9) and spread(10)
f.j_position = [0.0, 0.0, 0.0, 0.0]
def publish_messages(self, V_translation, V_rotation, terrain_grid_points,
V_viz_points, frame_J1, frame_J2):
"""
Publishes the pose stamped, multi-array, point-cloud and vehicle footprint vizualization
marker.
"""
##################################################################################
# Create a posestamped message containing position information
# Create pose message
msg = PoseStamped()
# Header details for pose message
msg.header.frame_id = "map"
msg.header.stamp = rospy.Time.now()
# Pose information
msg.pose.position.x = V_translation[0]
msg.pose.position.y = V_translation[1]
msg.pose.position.z = V_translation[2]
msg.pose.orientation.x = V_rotation[0]
msg.pose.orientation.y = V_rotation[1]
msg.pose.orientation.z = V_rotation[2]
msg.pose.orientation.w = V_rotation[3]
##################################################################################
# Create an multi array message containing pose information
# Create array message
array_msg = Float32MultiArray()
array_msg.layout.dim.append(MultiArrayDimension())
array_msg.layout.dim[0].label = "vehicle_position"
array_msg.layout.dim[0].size = 3
array_msg.layout.dim[0].stride = 3
# Append data
array_msg.data.append(V_translation[0])
array_msg.data.append(V_translation[1])
array_msg.data.append(V_translation[2])
##################################################################################
# Create point cloud and publish to rviz
# Create a point cloud from the xyz values in the array list
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
point_cloud = pcl2.create_cloud_xyz32(header, terrain_grid_points)
##################################################################################
# Create a marker to vizualize the footprint of the vehicle
viz_points = Marker()
viz_points.header.frame_id = "map"
viz_points.header.stamp = rospy.Time.now()
viz_points.ns = "grid_marker"
viz_points.id = 1
viz_points.action = viz_points.ADD
viz_points.type = viz_points.CUBE_LIST
viz_points.scale.x = 0.01
viz_points.scale.y = 0.01
viz_points.scale.z = 0.01
viz_points.color.a = 1.0
viz_points.color.r = 1.0
viz_points.color.g = 0.0
viz_points.color.b = 0.0
viz_points.points = V_viz_points
################################################################
# Create pose message for joints 1 & 2
msg1 = PoseStamped()
msg2 = PoseStamped()
# Header details for pose message
msg1.header.frame_id = "vehicle_frame"
msg1.header.stamp = rospy.Time.now()
msg2.header.frame_id = "vehicle_frame"
msg2.header.stamp = rospy.Time.now()
# Pose information
joint_1 = tf_conversions.toMsg(frame_J1)
joint_2 = tf_conversions.toMsg(frame_J2)
msg1.pose = joint_1
msg2.pose = joint_2
# Publish pose, vizualization, array information and point cloud
self.pose_publisher.publish(msg)
self.joint1_pose_publisher.publish(msg1)
self.joint2_pose_publisher.publish(msg2)
self.pose_array_publisher.publish(array_msg)
self.point_cloud_publisher.publish(point_cloud)
self.grid_publisher.publish(viz_points)