Exemplo n.º 1
0
def yawToQuat(angle):
    quatList = quaternion_from_euler(0.0,0.0,angle)
    quat = QuaternionMsg()
    quat.x = quatList[0]
    quat.y = quatList[1]
    quat.z = quatList[2]
    quat.w = quatList[3]
    return quat
Exemplo n.º 2
0
def yawToQuat(angle):
    quatList = quaternion_from_euler(0.0, 0.0, angle)
    quat = QuaternionMsg()
    quat.x = quatList[0]
    quat.y = quatList[1]
    quat.z = quatList[2]
    quat.w = quatList[3]
    return quat
Exemplo n.º 3
0
 def createQuat(x,y,z):
     quatList = quaternion_from_euler(x,y,z)
     quat = QuaternionMsg()
     quat.x = quatList[0]
     quat.y = quatList[1]
     quat.z = quatList[2]
     quat.w = quatList[3]
     return quat
Exemplo n.º 4
0
def createQuat(x, y, z):
    quatList = quaternion_from_euler(x, y, z)
    quat = QuaternionMsg()
    quat.x = quatList[0]
    quat.y = quatList[1]
    quat.z = quatList[2]
    quat.w = quatList[3]
    return quat
Exemplo n.º 5
0
 def get_message(self):
     object_id = self.object_name.split("_")[-1]
     msg = ModelDescription()
     # generate InstanceId
     msg.instance_id = InstanceId()
     msg.instance_id.class_name = self.object_type
     msg.instance_id.id = object_id
     # HACK this information should be in some ontology
     if 'ProductWithAN' in self.object_type:
         msg.instance_id.ns = '/IAISupermarket/Catalog'
     elif 'ShelfLabel' in self.object_type:
         msg.instance_id.ns = '/IAISupermarket/ShelfLabels'
     else:
         msg.instance_id.ns = '/IAISupermarket/Shelves'
     # generate MeshDescription
     # NOTE: default is to use the class name
     msg.mesh_description = MeshDescription()
     msg.mesh_description.path_to_mesh = ''
     msg.mesh_description.path_to_material = ''
     # generate Tag's
     msg.tags.append(self.get_tag('SemLog', 'LogType', 'Static'))
     msg.tags.append(self.get_tag('SemLog', 'Id', object_id))
     msg.tags.append(self.get_tag('SemLog', 'Class', self.object_type))
     # set the pose
     msg.pose = Pose()
     msg.pose.position = Point()
     msg.pose.position.x = self.transform[2][0]
     msg.pose.position.y = self.transform[2][1]
     msg.pose.position.z = self.transform[2][2]
     msg.pose.orientation = Quaternion()
     msg.pose.orientation.x = self.transform[3][0]
     msg.pose.orientation.y = self.transform[3][1]
     msg.pose.orientation.z = self.transform[3][2]
     msg.pose.orientation.w = self.transform[3][3]
     return msg
Exemplo n.º 6
0
    def __init__(self):
        """Class printing the marker of the wheelchair in rviz
        """
        """ MEMBERS """
        self.markers = UI_Markers()
        self.user_dir = Quaternion(
        )  # The user commanded direction, it can come from keyboard = key_cmd, voice = voice_cmd, or head =  head_cmd
        self.user_vel = Twist(
        )  # The user commanded direction, it can come from keyboard = key_cmd, voice = voice_cmd, or head =  head_cmd
        """ROS PARAMETERS"""
        self.tf_prefix = rospy.get_param(
            'tf_prefix', '')  # # Reads the tf_prefix from the ROS namespace
        if self.tf_prefix is not '':
            self.tf_prefix = '/' + self.tf_prefix
        self.discrete_interface = rospy.get_param(
            '~discrete_interface', False
        )  ## To enable/disable the discrete mode Set true when using the keyboard
        """ ROS SUBSCRIBERS """
        self.user_dir_sub = rospy.Subscriber("user_dir", Quaternion,
                                             self.__user_dir_callback__, None,
                                             1)
        self.user_vel_sub = rospy.Subscriber("user_vel", Twist,
                                             self.__user_vel_callback__, None,
                                             1)
        self.vocal_command_sub = rospy.Subscriber("recognizer/output", String,
                                                  self.__voice_callback__,
                                                  None, 1)
        """ROS PUBLISHERS"""
        self.wheelchair_marker_pub = rospy.Publisher("wheelchair_marker",
                                                     Marker,
                                                     queue_size=1)
        self.humans_marker_array_pub = rospy.Publisher("humans_marker_array",
                                                       MarkerArray,
                                                       queue_size=1)
        self.dir_marker_pub = rospy.Publisher("dir_marker",
                                              Marker,
                                              queue_size=1)
        self.vel_marker_pub = rospy.Publisher("vel_marker",
                                              Marker,
                                              queue_size=1)
        self.text_marker_pub = rospy.Publisher('text_marker',
                                               Marker,
                                               queue_size=1)
        """Start
        """
        r = rospy.Rate(10.0)

        while not rospy.is_shutdown():

            wheelchair_marker = self.fill_wheelchair_marker()

            humans = self.fill_humans_marker_array()
            self.wheelchair_marker_pub.publish(wheelchair_marker)
            self.wheelchair_marker_pub.publish(self.fill_sit_human_marker(
            ))  # I send it in the same topic it works well.
            self.humans_marker_array_pub.publish(humans)
            self.print_vel()

            r.sleep()
 def goto_shelf2(self):
     shelf = PoseStamped()
     header = Header()
     header.frame_id = 'map'
     shelf.header = header
     shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0)
     shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
     self(shelf)
 def relative_pose(self, position, orientation):
     shelf = PoseStamped()
     header = Header()
     header.frame_id = 'base_footprint'
     shelf.header = header
     shelf.pose.position = Point(*position)
     shelf.pose.orientation = Quaternion(*orientation)
     self(shelf)
Exemplo n.º 9
0
 def parse_pose(pose):
     rospy.loginfo('parse_pose')
     f_pose = Pose()
     f_pose.position.x = pose[0]
     f_pose.position.y = pose[1]
     f_pose.position.z = pose[2]
     f_pose.orientation = Quaternion(*quaternion_from_euler(pose[3], pose[4], pose[5]))
     return f_pose
 def goto_shelf4(self):
     shelf = PoseStamped()
     header = Header()
     header.frame_id = 'map'
     shelf.header = header
     shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000)
     shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
     # shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99))
     self(shelf)
 def goto_shelf1(self):
     # self.client.cancel_all_goals()
     shelf = PoseStamped()
     header = Header()
     header.frame_id = 'map'
     shelf.header = header
     shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0)
     shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
     self(shelf)
Exemplo n.º 12
0
    def add_cam_mast(self, x, y):
        """
        Adds a Cylinder where the cam mast is.
        :param x: x coordinate
        :type: float
        :param y: y coordinate
        :type: float
        """
        pose = PoseStamped()
        pose.header.frame_id = "/odom_combined"
        pose.pose.position = Point(x, y, 0.55)
        pose.pose.orientation = Quaternion(0, 0, 0, 1)
        c1 = self.make_cylinder("cm1", pose, [1.1, 0.05])
        self.safe_objects.append(c1.id)
        self.add_object(c1)

        pose.pose.position = Point(x, y, 1.0)
        pose.pose.orientation = Quaternion(0, 0, 0, 1)
        c1 = self.make_box("mast_cams", pose, [0.3, 0.3, 0.3])
        self.safe_objects.append(c1.id)
        self.add_object(c1)
Exemplo n.º 13
0
def rotate_quaternion_by_quaternion(q1, q2):
    """
    Rotates a quaternion by another quaternion
    :param q1: first Quaternion
    :type: Quaternion
    :param q2: second Quaternion
    :type: Quaternion
    :return: rotated Quaternion
    :type: Quaternion
    """
    r = quaternion_multiply([q1.x, q1.y, q1.z, q1.w], [q2.x, q2.y, q2.z, q2.w])
    return Quaternion(*r)
Exemplo n.º 14
0
 def add_ground(self, height=-0.01):
     """
     Adds a big flat box to the planning scene.
     :param height: z value
     :type: float
     """
     pose = PoseStamped()
     pose.header.frame_id = "/odom_combined"
     pose.pose.position = Point(0, 0, height)
     pose.pose.orientation = Quaternion(0, 0, 0, 1)
     box = self.make_box("ground" + str(height), pose, [3, 3, 0.01])
     self.safe_objects.append(box.id)
     self.add_object(box)
Exemplo n.º 15
0
def euler_to_quaternion(roll, pitch, yaw):
    """
    Creates a quaternion out of roll, pitch, yaw
    :param roll:
    :type: float
    :param pitch:
    :type: float
    :param yaw:
    :type: float
    :return: Quaternion from roll pitch yaw
    :type: Quaternion
    """
    return Quaternion(*quaternion_from_euler(roll, pitch, yaw))
Exemplo n.º 16
0
    def test2_1(self):

        co = CollisionObject()
        co.operation = CollisionObject.ADD
        co.id = "muh"
        co.header.frame_id = "/odom_combined"
        cylinder = SolidPrimitive()
        cylinder.type = SolidPrimitive.CYLINDER
        cylinder.dimensions.append(0.3)
        cylinder.dimensions.append(0.03)
        co.primitives = [cylinder]
        co.primitive_poses = [Pose()]
        co.primitive_poses[0].position = Point(1.2185, 0,0)
        co.primitive_poses[0].orientation = Quaternion(0,0,0,1)

        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions.append(0.1)
        box.dimensions.append(0.1)
        box.dimensions.append(0.1)
        co.primitives.append(box)
        co.primitive_poses.append(Pose())
        co.primitive_poses[1].position = Point(1.1185, 0,0)
        co.primitive_poses[1].orientation = Quaternion(0,0,0,1)

        co.primitives.append(box)
        co.primitive_poses.append(Pose())
        co.primitive_poses[2].position = Point(0, 0,0)
        co.primitive_poses[2].orientation = Quaternion(0,0,0,1)


        p = PoseStamped()
        p.header.frame_id = "/odom_combined"
        p.pose.position = Point(1,0,0)
        p.pose.orientation = euler_to_quaternion(0,0,0)
        self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
Exemplo n.º 17
0
def prolog_to_transform(frame_id, child_frame_id, translation, rotation):
    """
    :type frame_id: str
    :type child_frame_id: str
    :type translation: list
    :type rotation: list
    :rtype: TransformStamped
    """
    if child_frame_id==None or translation==None or rotation==None: return None
    t = TransformStamped()
    t.header.frame_id = frame_id.encode('utf-8')
    t.child_frame_id = child_frame_id.encode('utf-8')
    t.transform.translation = Point(*translation)
    t.transform.rotation = Quaternion(*rotation)
    return t
 def get_marker(self):
     marker = Marker()
     marker.header.frame_id = self.ref_frame
     marker.type = marker.MESH_RESOURCE
     marker.action = Marker.ADD
     marker.id = 1337
     marker.ns = self.get_short_name()
     marker.color = self.color
     marker.scale.x = 1
     marker.scale.y = 1
     marker.scale.z = 1
     marker.frame_locked = True
     marker.pose.position = Point(*self.transform[-2])
     marker.pose.orientation = Quaternion(*self.transform[-1])
     marker.mesh_resource = self.mesh_path[:-4] + '.dae'
     return marker
Exemplo n.º 19
0
def rotate_quaternion(q, roll, pitch, yaw):
    '''
    Rotates a quaternion by roll, pitch, yaw.
    :param q: Quaternion
    :type: Quaternion
    :param roll: roll
    :type: float
    :param pitch: pitch
    :type: float
    :param yaw: yaw
    :type: float
    :return: rotated quaternion
    :type: Quaternion
    '''
    angle = quaternion_from_euler(roll, pitch, yaw)
    no = quaternion_multiply([q.x, q.y, q.z, q.w], angle)
    return Quaternion(*no)
Exemplo n.º 20
0
    def addConveyorBelt(self, msg):
        print "addConveyorBelt callback"
        # print msg
        # zu 1
        drop_point = msg.conveyor_belt.drop_center_point
        print drop_point
        # zu 2
        conveyor_belt_width = msg.conveyor_belt.drop_deviation.y * 2
        print conveyor_belt_width
        # zu 3
        conveyor_belt_height = drop_point.z
        print conveyor_belt_height
        # zu 4
        conveyor_belt_length = msg.conveyor_belt.move_direction_and_length.y
        print conveyor_belt_length

        pose = PoseStamped()
        pose.header.frame_id = "/odom_combined"
        # h w d
        # pose.pose.position = Point(drop_point.x,
        # (drop_point.y - abs(conveyor_belt_length / 2)),
        # drop_point.z - abs(conveyor_belt_height / 2))
        # pose.pose.orientation = Quaternion(0, 0, 0, 1)

        l = math.sqrt(msg.conveyor_belt.move_direction_and_length.x *
                      msg.conveyor_belt.move_direction_and_length.x +
                      conveyor_belt_length * conveyor_belt_length)
        alpha = math.pi / 2 - math.asin(conveyor_belt_length / l)
        wx = conveyor_belt_width * math.cos(alpha)
        wy = conveyor_belt_width * math.sin(alpha)
        w = wx + wy

        pose.pose.position = Point(
            drop_point.x, (drop_point.y - abs(conveyor_belt_length / 2)),
            drop_point.z - abs(conveyor_belt_height / 2))
        pose.pose.orientation = Quaternion(0, 0, 0, 1)

        print pose

        cb_size = [w * 2, l, conveyor_belt_height]

        co = utils.manipulation.get_planning_scene().make_box(
            "conveyor_belt", pose, cb_size)
        utils.manipulation.get_planning_scene().add_object(co)
Exemplo n.º 21
0
    def get_marker(self):
        marker = Marker()
        marker.header.frame_id = self.ref_frame
        marker.type = marker.MESH_RESOURCE
        marker.action = Marker.ADD
        marker.id = 1337
        marker.ns = self.object_name
        marker.color = self.color

        marker.scale.x = 1
        marker.scale.y = 1
        marker.scale.z = 1
        marker.frame_locked = True
        marker.pose.position = Point(*self.transform[-2])
        marker.pose.orientation = Quaternion(*self.transform[-1])
        if len(self.mesh_path) > 2 and self.mesh_path[0] == "'":
            marker.mesh_resource = self.mesh_path[1:-1]
        else:
            marker.mesh_resource = self.mesh_path
        return marker
Exemplo n.º 22
0
def three_points_to_quaternion(origin, to, roll=None):
    """
    Calculates a quaternion that points from "origin" to "to" and lies in the plane defined by "origin", "to" and "roll".
    :param origin: form this point
    :type: Point
    :param to: to this point
    :type: Point
    :param roll: in a plane defined by this third point
    :type: Point or None
    :return: orientation
    :type: Quaternion
    """
    muh = False
    if roll is None:  #TODO buggy
        roll = Point(0, 0, origin.z + 1.000001)
        muh = True
    n_1 = subtract_point(to, origin)
    n_1 = normalize(n_1)

    n = subtract_point(roll, origin)
    n = normalize(n)

    n_2 = subtract_point(n, multiply_point(dot_product(n, n_1), n_1))
    n_2 = normalize(n_2)

    n_3 = cross_product(n_1, n_2)
    n_3 = normalize(n_3)

    rm = np.array(((n_1.x, n_2.x, n_3.x, 0), (n_1.y, n_2.y, n_3.y, 0),
                   (n_1.z, n_2.z, n_3.z, 0), (0, 0, 0, 1)),
                  dtype=np.float64)

    q = quaternion_from_matrix(rm)
    q = Quaternion(*q)
    if muh:
        q = rotate_quaternion(q, -pi / 2, 0, 0)
    return q
Exemplo n.º 23
0
    def make6DofMarker(self, interaction_mode, root_link, tip_link):
        def normed_q(x, y, z, w):
            return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])

        int_marker = InteractiveMarker()

        p = PoseStamped()
        p.header.frame_id = tip_link
        p.pose.orientation.w = 1

        int_marker.header.frame_id = tip_link
        int_marker.pose.orientation.w = 1
        int_marker.pose = self.transformPose(root_link, p).pose
        int_marker.header.frame_id = root_link
        int_marker.scale = .2

        int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)

        # insert a box
        self.makeBoxControl(int_marker)
        int_marker.controls[0].interaction_mode = interaction_mode

        if interaction_mode != InteractiveMarkerControl.NONE:
            control_modes_dict = {
                InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
                InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
                InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
            }
            int_marker.name += "_" + control_modes_dict[interaction_mode]

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(0, 0, 0, 1)
        control.name = "rotate_x"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(0, 0, 0, 1)
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
        control.name = "rotate_z"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
        control.name = "rotate_y"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        return int_marker
Exemplo n.º 24
0
    def make_6dof_marker(self, interaction_mode, root_link, tip_link):
        def normed_q(x, y, z, w):
            return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])

        int_marker = InteractiveMarker()

        int_marker.header.frame_id = tip_link
        int_marker.pose.orientation.w = 1
        int_marker.scale = MARKER_SCALE

        int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link)

        # insert a box
        self.make_sphere_control(int_marker)
        int_marker.controls[0].interaction_mode = interaction_mode

        if interaction_mode != InteractiveMarkerControl.NONE:
            control_modes_dict = {
                InteractiveMarkerControl.MOVE_3D: u'MOVE_3D',
                InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D',
                InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D'
            }
            int_marker.name += u'_' + control_modes_dict[interaction_mode]

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(0, 0, 0, 1)
        control.name = u'rotate_x'
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(0, 0, 0, 1)
        control.name = u'move_x'
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
        control.name = u'rotate_z'
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
        control.name = u'move_z'
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
        control.name = u'rotate_y'
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
        control.name = u'move_y'
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)
        self.markers[int_marker.name] = int_marker
        return int_marker
Exemplo n.º 25
0
 def pub_goal_marker(self, header, pose):
     """
     :param header:
     :type header: std_msgs.msg._Header.Header
     :param pose:
     :type pose: Pose
     """
     ma = MarkerArray()
     m = Marker()
     m.action = Marker.ADD
     m.type = Marker.CYLINDER
     m.header = header
     old_q = [
         pose.orientation.x, pose.orientation.y, pose.orientation.z,
         pose.orientation.w
     ]
     # x
     m.pose = deepcopy(pose)
     m.scale.x = 0.05 * MARKER_SCALE
     m.scale.y = 0.05 * MARKER_SCALE
     m.scale.z = MARKER_SCALE
     muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
     m.pose.position.x += muh[0]
     m.pose.position.y += muh[1]
     m.pose.position.z += muh[2]
     m.pose.orientation = Quaternion(*quaternion_multiply(
         old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
     m.color.r = 1
     m.color.g = 0
     m.color.b = 0
     m.color.a = 1
     m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
                                               self.tip_link)
     m.id = 0
     ma.markers.append(m)
     # y
     m = deepcopy(m)
     m.pose = deepcopy(pose)
     muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
     m.pose.position.x += muh[0]
     m.pose.position.y += muh[1]
     m.pose.position.z += muh[2]
     m.pose.orientation = Quaternion(*quaternion_multiply(
         old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
     m.color.r = 0
     m.color.g = 1
     m.color.b = 0
     m.color.a = 1
     m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
                                               self.tip_link)
     m.id = 1
     ma.markers.append(m)
     # z
     m = deepcopy(m)
     m.pose = deepcopy(pose)
     muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
     m.pose.position.x += muh[0]
     m.pose.position.y += muh[1]
     m.pose.position.z += muh[2]
     m.color.r = 0
     m.color.g = 0
     m.color.b = 1
     m.color.a = 1
     m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
                                               self.tip_link)
     m.id = 2
     ma.markers.append(m)
     self.marker_pub.publish(ma)
Exemplo n.º 26
0
    joint_state.position = [
        0.0174419, -1.33713, 1.47922, -1.05844, -0.00214559, -0.660072
    ]
    #                            0.0138173009806,
    #                            -0.948155685642,
    #                            1.64415424887,
    #                            0.833014565285,
    #                            -0.00345735390984,
    #                            -1.52754142152, ]
    test.send_joint_goal(joint_state)

    goal = PoseStamped()
    goal.header.frame_id = 'gripper_tool_frame'
    goal.pose.position.x = 0.1
    goal.pose.position.z = -0.2
    goal.pose.orientation.w = 1.0
    test.send_cart_goal(goal)

    goal = PoseStamped()
    goal.header.frame_id = 'gripper_tool_frame'
    goal.pose.position = Point(0, 0, 0)
    goal.pose.orientation = Quaternion(0.0, 0.0, 0.70710678, 0.70710678)
    test.send_cart_goal(goal)

    goal = PoseStamped()
    goal.header.frame_id = 'gripper_tool_frame'
    goal.pose.position = Point(0, -0.1, 0)
    goal.pose.orientation = Quaternion(-0.35355339, -0.35355339, -0.14644661,
                                       0.85355339)
    test.send_cart_goal(goal)
Exemplo n.º 27
0
def matrixToPose(matrix):
    t = tuple(translation_from_matrix(matrix))
    q = tuple(quaternion_from_matrix(matrix))

    return Pose(Point(t[0], t[1], t[2]), Quaternion(q[0], q[1], q[2], q[3]))
Exemplo n.º 28
0
    def wiggle_cb(self, goal):
        # check for invalid goals
        if goal.arm != WiggleGoal.LEFT_ARM and goal.arm != WiggleGoal.RIGHT_ARM:
            rospy.logerr('arm goal should be {} or {}'.format(
                WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
            return self.wiggle_action_server.set_aborted(
                text='arm goal should be {} or {}'.format(
                    WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
        if np.linalg.norm(
                np.array([
                    goal.goal_pose.pose.orientation.x,
                    goal.goal_pose.pose.orientation.y,
                    goal.goal_pose.pose.orientation.z,
                    goal.goal_pose.pose.orientation.w
                ])) < 0.99:
            rospy.logerr('invalid orientation in goal position')
            return self.wiggle_action_server.set_aborted(
                text='invalid orientation in goal position')

        rospy.loginfo('wiggle wiggle wiggle')
        movement_duration = goal.timeout.data.to_sec()
        wiggle_per_sec = goal.cycle_time
        number_of_wiggles = int(movement_duration / wiggle_per_sec)
        number_of_wiggle_points = 10
        radius_x = goal.upperbound_x
        radius_y = goal.upperbound_y
        hz = number_of_wiggle_points / wiggle_per_sec

        r = rospy.Rate(hz)

        tcp_goal_pose = self.transformPose('left_gripper_tool_frame',
                                           goal.goal_pose)
        spiral = self.create_spiral(goal_height=tcp_goal_pose.pose.position.z,
                                    radius_x=radius_x,
                                    radius_y=radius_y,
                                    number_of_points=number_of_wiggle_points,
                                    number_of_wiggles=number_of_wiggles)
        header = Header()
        header.frame_id = 'left_gripper_tool_frame'
        pose_list = self.np_array_to_pose_stampeds(spiral,
                                                   Quaternion(0, 0, 0, 1),
                                                   header)
        pose_list = [
            deepcopy(self.transformPose('base_footprint', pose))
            for pose in pose_list
        ]

        end_pose = self.transformPose('base_footprint', goal.goal_pose)

        for i, pose in enumerate(pose_list):
            if self.check_for_stop():
                return
            self.pub_wiggle(pose, goal.arm)
            wiggle_feedback = WiggleFeedback()
            wiggle_feedback.progress = .9 * (i + 1) / float(len(pose_list))
            self.wiggle_action_server.publish_feedback(wiggle_feedback)
            r.sleep()

        # move back to real goal
        time_to_get_to_endpose = rospy.Duration(1.0)
        deadline = rospy.get_rostime() + time_to_get_to_endpose
        while rospy.get_rostime() < deadline:
            if self.check_for_stop():
                return
            self.pub_wiggle(end_pose, goal.arm)

        result = WiggleFeedback()
        result.progress = 1.
        self.wiggle_action_server.publish_feedback(result)
        self.wiggle_action_server.set_succeeded(result)