def vtkFrameToPoseMsg(self, vtkFrame):
poseDict = spartanUtils.poseFromTransform(vtkFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
return poseStamped
def makePoseStampedFromGraspFrame(self, graspFrame):
iiwaLinkEEFrame = self.getIiwaLinkEEFrameFromGraspFrame(graspFrame)
poseDict = spartanUtils.poseFromTransform(iiwaLinkEEFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
return poseStamped
def moveToFrame(self, graspFrame):
iiwaLinkEEFrame = self.getIiwaLinkEEFrameFromGraspFrame(graspFrame)
poseDict = spartanUtils.poseFromTransform(iiwaLinkEEFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
self.poseStamped = poseStamped
self.robotService.moveToCartesianPosition(poseStamped,
self.config['grasp_speed'])
def testMoveToFrame(self):
pos = [0.51148583, 0.0152224, 0.50182436]
quat = [0.68751512, 0.15384615, 0.69882778, -0.12366916]
targetFrame = transformUtils.transformFromPose(pos, quat)
poseDict = spartanUtils.poseFromTransform(targetFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
self.poseStamped = poseStamped
self.robotService.moveToCartesianPosition(poseStamped, 30)
def makePoseStampedFromGraspFrame(self, graspFrame):
"""
Make PoseStamped message for the end effector frame from a given grasp frame
:param graspFrame: vtkTransform of the gripper frame
:return : pose of the end-effector for that grasp frame location
:rtype : geometry_msgs/PoseStamped
"""
iiwaLinkEEFrame = self.getIiwaLinkEEFrameFromGraspFrame(graspFrame)
poseDict = spartanUtils.poseFromTransform(iiwaLinkEEFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
return poseStamped
def __init__(self, tf, height, radius, name, color, im_server, listwidget,
remove_callback):
self.tf = tf.copy()
self.height = height
self.radius = radius
self.color = color
self.name = name
# Users provide feedback to refine the mesh
# position + configuration through RViz
# InteractiveMarkers
self.im_marker = ros_im.InteractiveMarker()
self.im_marker.header.frame_id = "base"
self.im_marker.name = name
self.im_marker.description = "Hypothesized Carrot"
self.im_marker.scale = 0.15
self.im_marker.pose = ros_utils.ROSPoseMsgFromPose(
spartanUtils.dict_from_homogenous_transform(tf))
# Visualize current carrot mesh
self.mesh_control = None
self._regenerateMesh()
# Add some control widgets
self._add6DofControls()
self.im_server = im_server
self.im_server.insert(self.im_marker, self._processImMarkerFeedbackCb)
self.im_server.applyChanges()
# Make control interface + add to list widget
self.control_widget = CarrotHypothesisWidget(name=name,
radius=radius,
height=height,
owner_hypothesis=self)
self.control_widget_listwidgetitem = QListWidgetItem(listwidget)
self.control_widget_listwidgetitem.setSizeHint(
self.control_widget.sizeHint())
self.listwidget = listwidget
self.listwidget.addItem(self.control_widget_listwidgetitem)
self.listwidget.setItemWidget(self.control_widget_listwidgetitem,
self.control_widget)
self.remove_callback = remove_callback
def rectangleMessageFromYamlNode(node):
msg = spartan_grasp_msgs.msg.Rectangle()
msg.min_pt = rosUtils.listToPointMsg(node['min_pt'])
msg.max_pt = rosUtils.listToPointMsg(node['max_pt'])
msg.pose = rosUtils.ROSPoseMsgFromPose(node)
return msg
