Python geometry_msgs.msg.Quaternion() Examples

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.005))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.45
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def state_to_pose(xyz, quat):
    """
    :param xyz: desired pose xyz
    :param quat: quaternion around z angle in [w, x, y, z] format
    :return: stamped pose
    """
    quat = Quaternion_msg(
        w=quat[0],
        x=quat[1],
        y=quat[2],
        z=quat[3]
    )

    desired_pose = inverse_kinematics.get_pose_stamped(xyz[0],
                                                       xyz[1],
                                                       xyz[2],
                                                       quat)
    return desired_pose 

def state_to_pose(xyz, quat):
    """
    :param xyz: desired pose xyz
    :param quat: quaternion around z angle in [w, x, y, z] format
    :return: stamped pose
    """
    quat = Quaternion_msg(
        w=quat[0],
        x=quat[1],
        y=quat[2],
        z=quat[3]
    )

    desired_pose = inverse_kinematics.get_pose_stamped(xyz[0],
                                                       xyz[1],
                                                       xyz[2],
                                                       quat)
    return desired_pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.009))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.009))

        return p.pose 

def _CreateMoveBaseGoal(self, goal):
		'''
		Creates an instance of MoveBaseGoal based on a simple goal in the form of a (x,y,theta) tuple
		'''
		
		x,y,theta = goal
		
		moveBaseGoal = MoveBaseGoal()
		moveBaseGoal.target_pose.header.frame_id = self._GoalFrameId
		moveBaseGoal.target_pose.header.stamp = rospy.Time.now()

		moveBaseGoal.target_pose.pose.position.x = x
		moveBaseGoal.target_pose.pose.position.y = y
		
		quaternionArray = tf.transformations.quaternion_about_axis(theta, (0,0,1))
		# quaternion_about_axis offers a convenient way for calculating the members of a quaternion.
		# In order to use it we need to convert it to a Quaternion message structure
		moveBaseGoal.target_pose.pose.orientation = self.array_to_quaternion(quaternionArray)

		print(moveBaseGoal)
		return moveBaseGoal 

def _CreateMoveBaseGoal(self, goal):
		'''
		Creates an instance of MoveBaseGoal based on a simple goal in the form of a (x,y,theta) tuple
		'''
		
		x,y,theta = goal
		
		moveBaseGoal = MoveBaseGoal()
		moveBaseGoal.target_pose.header.frame_id = self._GoalFrameId
		moveBaseGoal.target_pose.header.stamp = rospy.Time.now()

		moveBaseGoal.target_pose.pose.position.x = x
		moveBaseGoal.target_pose.pose.position.y = y
		
		quaternionArray = tf.transformations.quaternion_about_axis(theta, (0,0,1))
		# quaternion_about_axis offers a convenient way for calculating the members of a quaternion.
		# In order to use it we need to convert it to a Quaternion message structure
		moveBaseGoal.target_pose.pose.orientation = self.array_to_quaternion(quaternionArray)

		print(moveBaseGoal)
		return moveBaseGoal 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.4
        p.pose.position.y = 0.0
        p.pose.position.z = 0.26

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.09))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.005))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.009))

        return p.pose 

def compute_grasp_pose_offset(self, pose):
        """
        :param pose:
        :return:
        """

        yrot = tf.transformations.quaternion_from_euler(0, math.pi, 0)

        cubeorientation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        # oorient = [overhead_orientation.x,overhead_orientation.y,overhead_orientation.z,overhead_orientation.w]

        # resultingorient = tf.transformations.quaternion_multiply(cubeorientation, tf.transformations.quaternion_conjugate(oorient))
        resultingorient = tf.transformations.quaternion_multiply(cubeorientation, yrot)

        # resultingorient = cubeorientation


        pose.orientation = Quaternion(x=resultingorient[0], y=resultingorient[1], z=resultingorient[2],
                                      w=resultingorient[3])

        pose.position.x += 0
        pose.position.y += 0
        pose.position.z = demo_constants.TABLE_HEIGHT + demo_constants.CUBE_EDGE_LENGTH
        return pose 

def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.09))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.009))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.005))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.009))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.45
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.09))

        return p.pose 

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose 

def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.009))

        return p.pose 

def on_enter(self, userdata):
        """Create and send action goal"""

        self._arrived = False
        self._failed = False

        # Create and populate action goal
        goal = MoveBaseGoal()

        pt = Point(x = userdata.waypoint.x, y = userdata.waypoint.y)
        qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta)

        goal.target_pose.pose = Pose(position = pt,
                                     orientation = Quaternion(*qt))

        goal.target_pose.header.frame_id = "odom"
        # goal.target_pose.header.stamp.secs = 5.0

        # Send the action goal for execution
        try:
            self._client.send_goal(self._action_topic, goal)
        except Exception as e:
            Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e))
            self._failed = True 

def trans_rot_to_hmat(trans, rot):
    '''
    Converts a rotation and translation to a homogeneous transform.

    **Args:**

        **trans (np.array):** Translation (x, y, z).

        **rot (np.array):** Quaternion (x, y, z, w).

    **Returns:**
        H (np.array): 4x4 homogenous transform matrix.
    '''
    H = transformations.quaternion_matrix(rot)
    H[0:3, 3] = trans
    return H 

def centroid_callback(self, data):
        self.goal_poses = []

        for blob in data.blobs:
            centroid = (blob.centroid.x, blob.centroid.y)
            
            if blob.axis is None or self.camera_model is None:
                return
            axis = numpy.concatenate((numpy.array(unmap(blob.axis.points[0])),\
                                      numpy.array(unmap(blob.axis.points[1]))))

            pos = self.solve_goal_pose(centroid)

            #Calculate desired orientation
            theta = self.calculate_angle(axis)
            quat = tf.transformations.quaternion_from_euler(-pi, 0, theta)

            self.goal_poses.append( Pose(position=Point(*pos), orientation=Quaternion(*quat)))
        self.done = True