simulate_state.py 文件源码

def set_cube_pose(self, name, pose, orient=None):
        """
        :param name: cube name, a string
        :param pose: cube position, a list of three float, [x, y, z]
        :param orient: cube orientation, a list of three float, [ix, iy, iz]
        :return:
        """
        self.cubes_pose.link_name = "cubes::" + name
        p = self.cubes_pose.pose
        cube_init = self.CubeMap[name]["init"]
        p.position.x = pose[0] - cube_init[0]
        p.position.y = pose[1] - cube_init[1]
        p.position.z = pose[2] - cube_init[2]
        if orient is None:
            orient = [0, 0, 0]
        q = quaternion_from_euler(orient[0], orient[1], orient[2])
        p.orientation = Quaternion(*q)
        self.pose_pub.publish(self.cubes_pose)